commit 1e69f94f38ed7f49e02ec69438ce4325cd172c0d
author Austin Schuh <austin.linux@gmail.com> Sat Nov 14 15:06:14 2020 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Nov 14 15:06:14 2020 -0800
tree ed3df23372e222fb6cb58b0629f2c8cbe702622a
parent 00e0908774b67cf417aa07bbcff7da29411e1721

Squashed 'third_party/allwpilib/' changes from 00e090877..66b57f032

66b57f032 [wpimath] Copy child constraint in region constraints (#2831)
cfac22b4c [wpilib] Reset odometry in path following examples (#2859)
2ef67f20a [wpilib] Add way to silence joystick connection warnings (#2845)
7a73946ce [build] Update OpenCV to remove WITH_GTK (#2856)
6d22b5a3c [wpigui] Render during resize events (#2857)
50050a0e5 [wpilibc] Update C++ DiffDriveSim example to match Java (#2839)
de1742279 [wpilib] Add IsJoystickConnected method (#2847)
6b5e83ce1 [wpilibj] DrivetrainSim: Initialize m_u to default value (#2854)
17d75d8a3 [wpilibj] SimDeviceSim: Make register device callbacks static (#2835)
616405f7a [wpilib] Fix DiffDriveSim pose reset and example (#2837)
5c2dc043c [wpilib] Update examples to export NewCommands (#2841)
24a3c12f3 [wpilib] Fix names and descriptions of examples (#2846)
3e544282f [hal] Use FPGA time in HAL_SendError (#2849)
3c85a4064 [sim] Use units for voltage and current in RoboRioSim (#2853)
ac3c336b9 [wpimath] Use units for LinearSystemId Kv and Ka (#2852)
f24f28244 [build] Disable Gazebo builds when -PmakeSim is not set (#2810)
0dfee4745 [wpiutil] netconsoleTee: Add option to specify port (#2840)
eb80f7a78 [wpilibc] SendableRegistry: Add range and null checks (#2830)
68fed2a1a [build] Update NativeUtils to 2021.0.4 (#2828)
10d118a8d Fix C++ gradle in OtherVersions.md (#2826)
e021c3319 [wpilib] Set AnalogPotentiometer dashboard type (#2825)
7b7548196 [wpilib] AnalogPotentiometer: provide scaled value to Dashboard (#2824)
e019c735e [build] Update compiler to 2021 (#2823)
c253f2c7e Update Readme to match current practice (NFC) (#2820)
0ce9133b5 [wpimath] Address issues with LinearSystemLoop reset() and matrix initialization (#2819)
6ac9683a3 [build] Fix Gradle flags for CI documentation job (#2817)
1d7739d8d [build] Build with -PreleaseMode on tag push (#2816)
1de2a6d85 [build] Fix release versioning (#2815)
0a723a50d [build] Use fetch-depth: 0 to get all history (#2812)
34b91318f [build] Add Developer ID signing workflow (#2779)
b11a7114a [build] Bump thirdparty-opencv to 3.4.7-4 (#2811)
2ca5e1c8d Update requirements in README to include full VS install and JDK (#2808)
7ae8c7b24 [sim] Use DutyCycleEncoder FPGA index for sim device number (#2803)
1e17e4086 [build] Add a javaFormat Gradle task (#2805)
1069019fd [wpilib] Add DutyCycleEncoderSim (#2798)
4422904a2 [build] Add WITH_GUI cmake option
5d085b78b [build] Fix cmake shared library build
0927c73b1 [build] Update NativeUtils to latest version (#2797)
5fe8f9017 [build] Refactor CMake flags (#2788)
5cdffeaba [sim] StepTiming(): incrementally step Notifiers in sequence (#2794)
6e7c7374f [build] Globally Exclude PMD.TooManyMethods (#2793)
fb7b41793 [wpilib] Add ADXRS450_GyroSim (#2800)
abbf9f01a [wpilib] Fix typos in simulation classes (#2799)
17698af5e [wpimath] Add distance/angle constructor to Translation2d (#2791)
b66fcdb3f [sim] Fix Field2D GUI crash when collapsed (#2786)
7fc48b75d [command] Add PIDSubsystem PIDController as child (#2784)
07ac5370d [wpimath] Fix m_nextR instantiation in LinearSystemLoop ctor (#2783)
7c8f1cf7a [wpilib] Support scheduling functions more often than robot loop (#2766)
57a97e3fb [wpilib] Remove WatchdogTest print statements (#2781)
061432147 [wpilib] Clean up physics simulation class APIs (#2763)
8f3e5794b [wpilib] Add TimedRobot unit tests (#2771)
a112b5e23 [wpilib] Fix ProfiledPIDController continuous input (#2652)
67859aea4 [build] Split documentation into its own job (#2775)
37643ab0b [wpimath] Use std::lower_bound in Trajectory::Sample() calculation (#2774)
b0ee11f7c [build] Update vcpkg actions to latest (#2776)
7647e29b2 Add docs for building robot projects with other WPILib versions (#2756)
a3e672f86 [wpimath] C++: Assign zero in MakeWhiteNoiseVector if std-dev is zero (#2773)
9058fe803 [wpilib] Update docs link to stable (NFC) (#2772)
32f429a81 [wpimath] Move DiscretizeR() in EKF and UKF from Predict() to Correct() (#2753)
bf2665654 [wpimath] Fix quintic spline generation from control vectors (#2762)
96e26247d [wpiutil] Add custom priority queue implementation (#2770)
8e538aa82 [wpilibc] Make IsSimulation() checks constexpr (#2769)
fa809b2c4 [wpilibc] Clean up include files (#2708)
9a63cd36c [wpilibc] Const-qualify Watchdog comparison operator (#2767)
21d949daa [wpilibc] Add LinearSystemLoop C++ ctor to match Java (#2755)
330b90e04 [wpilib] Enable ArmSubsystem in ArmBot example (#2752)
693daafe2 [wpilib] Rename LinearSystemSim's ResetState() to SetState() (#2750)
c3b3fb8b7 [sim] Change StepTiming to wait for notifiers (#2603)
62731bea2 [wpilib] Add set functions to differential drive simulation (#2746)
c55fb583b [wpilibj] Watchdog: Implement equals and hashCode (#2743)
9725aff83 [wpilib] Clean up DifferentialDrivetrainSim API (#2747)
1320691eb [wpilib] Shorten differential drive simulation stability test (#2745)
451f67c63 [hal] SimDevice class: remove HAL_SimDeviceHandle constructor (#2744)
43b1b128b Improve README-CMAKE.md (#2737)
fc991cb59 [wpilib] Clean up simulation physics API (#2739)
17d3d2f75 [wpilibc] Add ScopedTracer class (#2724)
73950b985 [wpilibc] RobotBase::IsReal, IsSimulation: Add doxygen comments (#2735)
61ee331f1 [build] Double gradle build max heap size to 2G (#2689)
651319589 [wpilibc] Fix a use-after-free in DifferentialDrivetrainSim (#2741)
5479948bd Update filepaths in thirdparty notices (#2713)
1ec145ec8 [imgui] Add IMGUI_IMPL_OPENGL_LOADER_GL3W to cmake compile defs (#2728)
8ab47cb07 [wpilib] Add C++ diff-drive sim tests and fix Java test name (#2729)
b7b3dcf3e Reorganize build documentation (#2712)
f7da0b452 Improve list of build requirements in README (#2709)
f9a338018 [build] Add macOS CMake CI (#2711)
b61f08d3f  [wpilib] Add physics simulation support with state-space  (#2615)
050322592 [build] CMake: Fix Metal linking on macOS (#2725)
d4d0b5501 [wpilib] Use more inclusive terminology (NFC) (#2723)
6c5726c96 Add CODE_OF_CONDUCT (#2716)
56972447b [wpigui] Only build static for both gradle and cmake (#2703)
67e1796ef [wpimath] Fix Eigen rule-of-three violations (#2707)
1ae8da3b3 [wpigui] Move portable-file-dialogs.h to wpigui and upgrade it (#2704)
3ed97f45f [cscore] Properly init variable in usbviewer (#2702)
ae2809cad [wpimath] Update KalmanFilter C++ class docs based on Java (NFC) (#2699)
b0a296477 [wpimath] Use newer link to controls book in comments (NFC) (#2700)
629a4574d [wpimath] Include Eigen headers in cppHeadersZip (#2696)
4be809499 [wpimath] Use more specific Eigen includes in headers (#2693)
b10063da9 [sim] Set Encoder reset to false when count set (#2692)
5e54e1314 [sim] Ignore incoming websocket DS/Joystick when real DS connected
8f87c5631 [sim] Automatically turn off GUI DS when real DS connected
1b18560e9 [sim] ds_socket: Remove iostream
f86a5f9b0 [sim] WebSockets: don't override HAL_Main
f1b1bdb12 [hal] Add HAL_Shutdown and simulation shutdown callbacks
0365557b2 [sim] Make extension registry thread-safe
883d96283 [wpiutil] SimpleBufferPool: Deallocate on destruction
35790a899 [wpimath] Remove rho overload from LinearQuadraticRegulator constructors (#2687)
8edc17dac [wpimath] Add Vector overload for times and div (#2686)
502f5c8b5 [wpimath] Make LinearPlantInversionFeedforward's A and B matrices final (#2688)
bc28fb187 [wpimath] Remove print from LinearSystemIDTest (#2684)
de0277713 [sim] Add API for extensions to discover each other (#2681)
1593eb4d4 [sim] Add plotting to simulation GUI
70ba92f91 [sim] GUI: Show DS info even when GUI DS disabled
47cc71ea0 [hal] Add HALSIM_GetJoystickCounts
9453d6727 [sim] Output more error details on extension load failure (#2680)
f758af826 [wpilib] Use misspell to find more spelling errors (NFC) (#2679)
6a1e5385e [sim] WS: Don't call virtual function from destructor
63487dca7 [sim] Fix WS blank device messages (e.g. DriverStation)
7d6f09f5c [sim] SimDataValue: Fix cancellation
148eed3cd [sim] Make sure NotifyListener members are initialized
05701317b [build] Make loading sim extensions from MyRobot easier (#2671)
7548fdae5 [hal] Fix InitialzeInterrupts naming (#2673)
3e41d92c1 [wpilib] Use misspell to fix spelling errors (NFC) (#2674)
ad6c8b882 [hal] Fix PDP Energy Calculation (#2672)
947ff655c [wpimath] Refactor KalmanFilter to be steady-state only (#2657)
183b7c85a [wpigui] Handle Direct3D framebuffer resize
4cf6947af [cscore] Add wpigui-based USB camera viewer
b83709b26 [wpigui] Add GetDistSquared and MaxFit functions
c699d5517 [wpigui] Build dev executable in cmake build
781afaa85 [wpigui] Refactor texture handling
007b03a2c [hal] Fix wpiformat errors (#2670)
ed1869334 [hal] Add no throw/error version of CAN Write methods (#2063)
7c99224bb Create maintainers.md (#1937)
c2c409090 [build] Fix CMake imgui and wpigui on macOS (#2669)
416288061 [build] Fix Gradle build on macOS 11.0 Big Sur (#2656)
83376bc23 [build] Add sourcelink support for windows pdbs (#2592)
c0de98f9f [sim] Fix GUI scaling of window sizes (#2668)
70db0db22 [build] Don't unconditionally regenerate wpimath numbers (#2665)
526f26685 [wpilib] Add methods to check game and enabled state together (#2661)
5d1220e62 [sim] Use wpigui for simulator GUI
b80fde438 [wpigui] Add wpigui wrappers for GLFW+imgui
148f43b4a [wpilibc] Use LLVM containers in ShuffleboardInstance (#2658)
0d88213de [sim] Add missing new line to extension loader message (#2663)
36b8d74fa [build] Revert "[build] Apply temporary JDK arch fix (#2643)" (#2655)
5021f2815 [wpilibc] Fix InterruptableSensorBase comment (#2659)
ee6a81457 [wpilib] Enable continuous input on theta controller in swerve examples (#2651)
807de9a0a [build] Fix Eigen include path for CMake install (#2649)
9398b6b55 [sim] Add AnalogEncoderSim (#2647)
932bfcf37 [sim] Add WebSocket extension (client/server) (#2589)
e127bac7f [sim] Properly initialize AnalogTrigger (#2646)
bccf13bf6 [build] Run unit tests in CMake build (#2644)
950bbd6dc [wpilibj] Fix joystick button edge synchronisation (#2433)
e680ba85f [build] Apply temporary JDK arch fix (#2643)
b23ede7d5 [wpimath] Remove unnecessary copying in constraints (#2645)
73047d8b3 [wpiutil] Fix WPIUtilJNI.now() implementation (#2640)
ef5e0c2e7 [wpimath] Remove duplicate WPIMathJNI class (#2639)
dc9e560f9 [sim] Add callback for NotifyNewData
ae5b07ba0 [hal] Add stubs for sim joysticks and match info on Rio
3384c23a5 [build] Specify --tags on fetch and add version sanity check (#2638)
c2259d42a [wpilib] Add Toggle() function to solenoid classes (#2635)
370e9d089 [build] Remove Azure Pipelines CI
cab8b18c6 [build] Add GitHub Actions CI Workflow
3b283ab9a [wpimath] Add core State-space classes (#2614)
e5b84e2f8 [wpimath] Use project includes for units, Drake, Eigen (#2634)
f86417d79 [wpimath] Move MathShared.h to wpimath/ directory
8dc3d2383 [wpimath] Move DrakeJNI to edu.wpi.first.math.WPIMathJNI
42993b15c [wpimath] Move math functionality into new wpimath library (#2629)
ad817d4f2 [sim] Map wpi::Now() to simulated FPGA time (#2631)
77954bb3d [wpiutil] Add JNI wrapper for wpi::Now() (#2632)
52f7a62e1 [wpiutil] Allow aarch64 platform on any OS (#2625)
1b8ceb36f [sim] Add callbacks for joysticks and match info (#2628)
ceea1f9d4 [sim] Add Mechanism2D to Simulation GUI (#2607)
2f81f2b78 [wpilib] Fix timestamp slew in SlewRateLimiter (#2627)
aba035eb3 [command] Modify swerve and mecanum commands to use new controller
5ca270208 [wpilib] Add HolonomicDriveController class and tests
af588adce [wpiutil] Add angle normalization method
399684a58 [wpilibc] Make SendableChooser movable (#2621)
5cf4c16f5 [wpilibj] Suppress serialVersionUID warnings (#2618)
0fe2319df [wpilibj] Use try with resources for Java tests (#2612)
e759dad01 [wpiutil] Add Drake and Drake JNI (#2613)
0c18abed3 [build] Double gradle build max heap size to 1G (#2616)
89dad2fd8 [build] Use artifactory doxygen mirror (#2600)
a6a71f8c7 [hal] Fix RoboRIO notifier never starting (#2611)
1fee190fd [wpiutil] Add MIME type utility (#2608)
23ba3ca19 [wpilib] Change Watchdog to use HAL notifier (#2602)
33d836329 [sim] Fix HAL notifier race condition (#2606)
49dcf7cf5 [command] Speed up MecanumControllerCommand and SwerveControllerCommand tests (#2604)
9778445a7 [sim] Change stepTiming to take seconds (#2599)
c2cc90b27 [sim] Move WPILib C++ sim implementations out of line (#2598)
b9feb8122 [sim] Add joystick simulation support (#2595)
b06ddcdd8 [wpilib] Improve and sync AnalogPotentiometer docs (NFC) (#2580)
16ef372b5 [hal] Merge WaitForCachedData into WaitForDSData (#2587)
f0a34ea64 [wpilib] Remove Analog Gyro specific docs from Gyro interface (#2596)
33f7dec5c [wpilibj] Fix Gyro.getRotation2d() units bug (#2594)
300580359 [build] Update to latest native utils (#2591)
eeaaf5d25 [build] Add Azure build using cmake on Windows (#2588)
15819cc98 [command] Add SimulationPeriodic to templates (#2582)
fe6bfb1ba [cscore] Provide USB Camera VID and PID in Windows (#2585)
5c8d7b242 [build] Remove install prefix from CMakeLists.txt for Windows (#2584)
ff0801d78 [build] Revert change to DeriveLineEnding made in #1961 (#2581)
ac4b0a311 [wpilib] AnalogPotentiometer: Use average voltage (#2583)
227084e92 [hal] Implement stub mockdata on RoboRIO (#2575)
a175f6e86 [command] Add simulationPeriodic method to Subsystem (#2577)
4043c461d [wpiutil] HttpServerConnection: Make more functions virtual (#2576)
b3b2aa635 [wpilib] Fix SplineHelper cubic spline bug (#2572)
5bd2dca46 [sim] Move HAL_LoadExtensions to end of HAL_Initialize (#2571)
21740fd2c [build] Add PDBs to jnicvstatic archives (#2573)
c11ef442f [wpiutil] Add HttpWebSocketServerConnection (#2505)
b5a38001d [cscore] Add support for changing the USB camera path (#2547)
5b6f68cf7 [wpilib] Add parentheses to MSVC #pragma message (#2569)
3050e935a [sim] Add WPILib-class-taking constructors (#2538)
80a1fa9ec [sim] Add support for disabling SimDevices (#2568)
1851ba143 [sim] Remove index from RoboRIO simulation interface (#2567)
c4b8a2505 [sim] Add Java wrapper for getProgramStarted (#2566)
a1d2d40ad [wpilib] Add RamseteController examples (#2553)
a3881bb45 [wpilibc] Add implicit conversion from degree_t to Rotation2d (#2564)
6e4ee8da2 [cscore] Limit jnicvstatic exports to only C and JNI symbols (#2565)
2a0f79b90 [wpilib] Add X and Y component getters to Pose2d and Transform2d (#2563)
5ccc98bc1 [wpiutil] Add angular acceleration units (#2562)
b1353e4d6 [cscore] Fix jnicvstatic classifier (#2561)
9f4de9155 [cscore] Add new jnicvstatic artifact type (#2560)
d30d1088d [wpiutil] Split units.h into separate headers for each unit (#2551)
c6e634664 [sim] Support StepTiming() even when timing is not paused (#2558)
e08c8a1fc [wpiutil] Add circular buffer class with static size (#2542)
e50dbe0c4 [wpilib] Add Gyro::GetRotation2d() (#2555)
d9c7bbd04 [wpiutil] Restore existing dll directory when setting dll directory (#2559)
dac0e5b13 [sim] Set Windows timer precision to 1ms (#2557)
ce3bc9194 [sim] Move Sim classes from HAL to wpilibc/j (#2549)
22c0e2813 [build] Upgrade CI to clang-format 10.0 (#1961)
9796987d5 [wpiutil] Add new combined native class loader (#2554)
4d275e476 [cscore] Fix USB camera test failure (#2556)
492d6c282 [sim] Update thirdparty-imgui (#2552)
765f00935 [wpiutil] Add HttpQueryMap and HttpPath/HttpPathRef (#2544)
629e8b41f [sim] Provide method to set the runtime type (#2539)
71c187a1e [hal] Add GetIndex() to handles classes (#2548)
00e991e2a [wpiutil] Add dual-IPv4/IPv6 uv::AddrToName() (#2545)
8a80f97c0 [hal] Move JNI helpers and sim namespace to hal namespace (#2543)
3cbec411c [build] Add package declaration check to checkstyle (#2446)
3bcb89fb8 Set rpath on all unix platforms to $ORIGIN (#2299)
62b8a36ce [sim] Wrap timing functions in C++ SimHooks (#2540)
27566abb0 [command] PIDSubsystem: Add GetSetpoint (#2534)
dfb130270 [wpilib] Use inclusive language where practical (#2533)
1ba616f84 [build] Fix 32-bit Windows builds (#2537)
11fb0a4cb [cscore] EnumerateUsbCameras(): Only list capture devices (#2532)
1557a4c3b [sim] Make SimDeviceInfo/SimValueInfo data members public (#2531)
ab28a7d65 [build] Run Gradle Wrapper validation on PRs (#2527)
d58a5e124 [sim] Name DSCommJoystickPacket structure (#2525)
762347f00 [wpilibj] Throw separate exception for constraint misbehavior (#2510)
4b76adf15 [wpilibc] Remove incorrect timer rollover (#2523)
6be1b9524 [wpilibj] Trajectory: Add zero-arg constructor (#2513)
38ad79061 CONTRIBUTING.md: Change Travis to Azure (#2520)
86acb27e2 [wpilibc] Fix doxygen comments (#2519)
1b395fa21 [wpilib] Implement Trajectory.equals() (#2517)
92380485c [command] Make cancel safe to call from initialize (#2440)
27f9a21a2 [wpilib] Moved Watchdog output to reportWarning() (#2413)
cf7088a46 [wpilibj] RamseteController: Fix units typo in javadocs (#2515)
67554ef3b [build] Add .inl files to styleguide (NFC) (#2506)
cf20b068c [sim] Map HAL_GetSystemActive to HALSIM_GetDriverStationEnabled (#2501)
0b9316d94 [build] Revert workaround for azure windows library loading issue (#2481) (#2499)
3011ebe54 [wpilibc] Fix uninitialized variable in Trajectory class (#2495)
4b77b0773 [wpilibj] SensorUtil: change exception type (#2490)
e5935a473 [wpilibc] Fix const-qualification in kinematics and constraints (#2478)
a3a8472b8 [wpilib] Trajectory: Add MaxVelocity and Region constraints (#2466)
212182d99 [wpilibj] Remove getSimObject (#2479)
c82b8546b [build] Work around azure windows library loading issue (#2481)
fac4e3fcf [wpilibc] Add real-time priority constructor to Notifier (#2303)
5b0122fed [sim] Add NetworkTables view to Simulation GUI (#2327)
b46b5df16 [wpilibc] Output Tracer to DriverStation by default (#2469)
cb5102933 [wpilib] Add Color.fromHSV() static helper (#2461)
e504b3ecb [command] Add NetworkButton (#2373)
0ad0ec098 [wpilibj] Move button tests to button package (#2472)
d1d32ada0 [build] Fix artifact publishing (#2471)
8058daa98 [sim] Add encoder rate and distance to Simulation GUI (#2470)
f4c5c0f5b [command] Add withName inline decorator to Java (#2420)
2ecb8dab7 Add issue templates (#2189)
576d427f0 [wpilib] SpeedControllerGroup: Add vector-taking constructor (#2194)
21aafea09 [hal] Use std::thread for Notifier thread on Rio (#2152)
3ed290856 [sim] Add HALSIM accessors for encoder rate and distance (#2467)
2b188b54d [ntcore] Always show StartLocal instances as connected (#2462)
306720e63 [build] Split build logic into separate files (#2457)
a308dd447 [wpilib] Fix SplineHelper quintic spline generation bug (#2429)
8f33d21bc [wpilib] Add init methods to Preferences (#2443)
b9ee3ae03 [wpilibc] Refactor Tracer functionality out of Watchdog class (#2456)
c14b87b22 [wpilibj] Refactor Tracer functionality out of Watchdog class (#2452)
8a279aaf2 [build] Check SHA-256 of downloaded Gradle distribution (#2169)
3a5e541b2 [wpilibc] Add doxygen deprecated tag to deprecated methods (#2336)
43574128b Add a CODEOWNERS file (#2431)
e4a990384 [build] Fix generateJavadoc dependencies (#2458)
0d30108dc [wpiutil] Endian.h: minor cleanup and improvements (#2454)
b7a79c70c C++: Add Watchdog to CommandScheduler (#2437)
6e6f28d1a Add watchdog and epoch reporting to CommandScheduler (#2319)
d14978e54 New commands: add docs to end() (#2450)
1c28b729a Move curvature_t to units namespace (#2444)
303194b08 Add missing CAN definition for IO Breakout (#2394)
2ee3bfaa2 Make SlewRateLimiter unit declarations public (#2445)
029a94dd3 Remove old simulation bits (ds_nt, lowfi, print) (#2432)
f6df9217b Remove static import usage in SelectCommand example (#2442)
184fae4ba DutyCycleEncoder: Set ownsDutyCycle for channel constructor (#2436)
35b236651 Timer.start(): Match C++ behavior in Java (#2434)
c92677055 DutyCycleEncoder: Close created DigitalInput (#2435)
272eaf184 DutyCycleEncoder: Fix simulation support (#2387)
56fbb1fc3 Make Ultrasonic.setAuto() static (#2419)
33f6bf947 DriverStation (Java): Make getStickButton functions public (#2424)
07326edb6 Sim GUI: Add user rename support to SimDevice list (#2426)
144610151 Bump NativeUtils to enable bigobj (#2430)
4228d3609 Fix Watchdog epoch prints being off by three orders of magnitude (#2414)
510936a2a Improve Button API documentation in GenericHID (#2421)
5854e284e Convert license file to Markdown (#2190)
a732606e5 Geometry classes: remove explicit serializers/deserializers (#2312)
84e300739 Fix ProfiledPIDController profile direction for continuous input (#2279)
8edf9282c Timer: synchronize on lock object (#2352)
5a1acaaef Update copyright dates for #2121 (#2415)
4fd7c210d Shuffleboard: Fixed mix up of title and type parameters (#2121)
a26a7d217 Remove references to percent tolerance (#2380)
7b7f44d93 Use netcomm function to get target class rather then hardcoded (#2391)
6cf89aa0f Fix interrupt cancellation (#2406)
3be83784c Add Transform2d::Inverse() (#2407)
b6c163acd Const-qualify frc2::Timer::HasElapsed() (#2397)
05a26b727 Fix missing Field2d package (#2386)
35eb90c13 Fix "'dimensionless' ambiguous symbol" error (#2382)
761f79385 Add SimulationInit and SimulationPeriodic functions (#2377)
554bda333 HAL: Expose NetComm SendConsoleLine (#2337)
2a968df77 Add method for releasing a waiting interrupt (#2347)
30ccd13b6 Add methods for getting color of an LED (#2366)
60c09ea51 Fix notifier race happening with very fast notifiers (#2370)
65eab9352 Fix DifferentialDriveVoltageConstraint for tight turns w/ zero velocity (#2341)
a226ad850 Set a default option for Sendable Chooser in examples (#2361)
31f4fd70c Sim GUI: Fix crash when field/robot image load is canceled (#2362)
7275ab983 Add headerpad_max_install_names to Mac builds (#2333)
5b3facc63 TrapezoidProfile: Make units public (#2342)
0f313fb9a cmake: Improved portability of OpenCV Java binding search (#2348)
05b7593e6 C++ circular_buffer: support types not implicitly convertible from int (#2350)
1b85066d2 Fix C++ ParallelRaceGroup multiple calls behavior (#2339)
e93b64f58 Add note that only a single instance of ADXRS450 is supported (#2349)
f0a18f31e Timer: add hasElapsed, advanceIfElapsed (#2322)
29c82527a Fix typo in ADXL sim double creation (#2332)
c165dc5e5 Simulation GUI: Add 2D field view (#2261)
42da07396 Add missing references to f in PIDSubsystem javadoc (#2318)
20e6c0405 Fix cmake install of hal generated files (#2320)
ff5d3e5b3 RamseteController: Add setEnabled method (#2313)
6cc68ab50 Update MavenArtifacts.md (#2289)
068465146 Fix null check order in SendableRegistry (#2314)
3bcf8057d Add more detail to RuntimeLoader failure message (#2309)
8039a6c52 Add missing include to ShuffleboardEventImportance (#2310)
558c020cc Fix duplicated state when using quintic splines (#2307)
7797da78f Add missing methods to Timed and TimedSkeleton templates (#2306)
0ab81d768 Add feed-forward and slew rate limiting to advanced drive examples (#2301)
1cee5ccb9 Feed RobotDrive watchdog in RamseteCommand example (#2298)
3ce01b5ac Fix DriveSubsystem.getHeading Java documentation (#2282)
e6aa8f3ff Update toolchain link in readme (#2304)
9d7b08797 Simulation GUI: Add support for custom names (#2292)
bb184ed48 Simulation GUI: Refactor ini saving (#2291)
b9b31069c Fix C++ POVButton (#2294)
d0cf4e888 Change sim Filesystem.getDeployDirectory() to src/main/deploy (#2293)
02fb85076 Add new speed controllers to shuffleboard docs (#2288)
ac8177e10 Fix GearsBot log methods not being called periodically (#2280)
2eb5c5447 TrapezoidProfile: Fix aliasing of m_initial to result in calculate() (#2284)
0e206e69c Remove Set Camera Server Shared message (#2285)
b1357cace Fix LiveWindow SetEnabled C++ std::bad_function_call (#2281)
37202b6f2 Add missing SensorUtil::kAnalogOutputs (#2276)
2ac0d5296 Remove AnalogTrigger::SetDutyCycle (#2275)
dbe1e6f46 Fix missing SetDutyCycleSimDevice on Rio (#2274)
a61fcbd68 Make Button class concrete (#2244)
fe597eeba Fix SPI DIO count for sim (#2272)
e213a47ef Official Gradle Wrapper Validation GitHub Action (#2273)
dcb96cb50 TrajectoryGenerator: Allow replacement of error reporting function (C++) (#2267)
60d48fec5 Fix Java static colors having zero values (#2269)
ee8475d21 Run wpiformat (#2270)
f47e31813 C++ units: Interoperate with Windows headers min/max (#2268)
cb66bcca3 Add callback handlers for LiveWindow (#2053)
73302f616 Fix color name typos (#2265)
cba21a768 Fix C++ JoystickButton and POVButton (#2259)
822e75ec4 Simulator GUI: Handle save file having window size=0 (#2260)
108ddfa1b Fix Pi Camera auto exposure property name (#2258)
d4c8ee591 Add Axis enum in XboxController (#2253)
ab9647ff5 CommandScheduler: Don't store NetworkTableEntry
6666d3be4 SendableBuilder: Allow multiple updateTable functions
795086b4c Fix Demangle when used standalone (#2256)
56765cf49 C++ CommandBase: Don't add to LiveWindow (#2255)
bf7012fa2 Fix new CommandScheduler.cancelAll() (#2251)
10e8fdb72 Make C++ IterativeRobotBase and RobotBase constructor and destructor public (#2242)
790dc552c Add quirks support for Pi camera (#2241)
0ec8ed6c0 Make C++ controller using declarations public (#2240)
832693617 Add missing definition for PIDController::SetPID() (#2239)
772ef8f96 Update Maven location to artifactory (#2235)
95b6cd2dd TrajectoryGenerator: Allow replacement of error reporting function (#2234)
ce1ac17df Remove experimental from new command example descriptions (#2226)
b2f7a6b65 Add clarification to LED about length and # of drivers (#2231)
bedbef799 Revert "Remove -no-module-directories flag from javadoc build (#2201)" (#2229)
bc159a92a Default sim voltage to 12v, make user rails active (#2224)
f50d710a5 Make color ctor public (#2222)
bc8f68bec Add sim HAL_WaitForCachedControlData (#2221)
32c62449b Add ArrayRef overloads to new command classes (#2216)
6190fcb23 Run wpiformat (#2218)
012d93b2b Use an explicit stack instead of recursion when parameterizing splines (#2197)
222669dc2 Fix trapezoidal profile PID controller setpoint bug (#2210)
abe25b795 TrajectoryUtil.toPathweaverJson: Create parent directories (#2214)
354185189 Update ProjectYear to 2020 (#2212)
f14fe434a Add (Old) qualifier to old subsystem (#2211)
e874ba931 Add Color classes for use with AddressableLED (#2127)
96348e835 Fix C++ SendableRegistry::AddChild() (#2207)
d91796f8d fix clang-format version number (#2206)
9abce8eb0 Fix subsystem LiveWindow usage (#2202)
8b4508ad5 Use default path for networktables.ini in simulation (#2205)
5b7dd186d Add templates for new commands for vscode plugin (#2016)
6ea13ea8f ntcore: Add support for local-only operation (#2204)
44bcf7fb4 Java examples: use non-static imports for constants (#2191)
c7a1dfc0b Add SlewRateLimiter class (#2192)
a12bb447e Fail cmake build if python3 generate_numbers.py fails (#2203)
c4bd54ef4 Add JNI binding to suppress driver station error/warning messages (#2200)
f9a11cce5 Remove -no-module-directories flag from javadoc build (#2201)
6008671c3 Report WPILib version as part of usage reporting (#2199)
7b952d599 Add usage reporting for many new things (#2184)
93cdf6869 Add Constants.cpp for MecanumControllerCommand example (#2196)
0c6f24562 Fix bug in ULEB128 decoding (#2195)
bdc1cab01 Add support for configuring SPI Auto Stall Config (#2193)
3259cffc6 Add transform methods to Trajectory (#2187)
67b59f2b3 Minor improvements/fixes to new command framework (#2186)
1ce24a7a2 Add 2020 speed controllers (#2188)
635882a9f Add getter for initial pose in Trajectory (#2180)
71a22861e Use ManagedStatic for CameraServer (#2174)
9cb69c5b4 Add a way to pass in a preconstructed value to ManagedStatic (#2175)
5e08bb28f Add docs and lifecycle tasks for faster dev builds (#2182)
ea4d1a39e Update characterization values to match real robot (#2183)
31b588d96 Fix ArmFeedforward Javadocs (#2176)
0b80d566a Use ChipObject HMB function for LED (#2173)
f8294e689 Sim GUI: Add a bit of spacing to the analog inputs (#2170)
b78f115fc Work around VS2019 16.4.0 bugs (#2171)
b468c5125 Change AddressableLED example to use consistent PWM port (#2168)
023c08829 Add toString() to relevant kinematics classes (#2160)
8a11d13a3 Fix C++ DutyCycleEncoder int constructor (#2166)
daa81c64a Minor javadoc fix in SwerveDriveKinematicsConstraint (#2167)
e20d96ea4 Use __has_include for WPILib.h (#2164)
a76d006a0 Update native-utils to 2020.7.2 (#2161)
24c031d69 Increase SPI auto byte count to allow 32 bytes to be sent (#2163)
6b4eecf5f Add hidden functions to get the SPI system and SPI DMA (#2162)
ccdd0fbdb Add TrapezoidProfile external PID examples (#2131)
5c6b8a0f4 Replace std::is_pod_v with std::is_standard_layout_v (#2159)
67d2fed68 Add DutyCycleEncoder channel constructor (#2158)
d8f11eb14 Hardcode channels for LSB weight (#2153)
b2ae75acd Add way to disable "no extensions found" message (#2134)
4f951789f Build testbench tests online inorder to improve speed (#2144)
005c4c5be Try catch around task dependencies to fix loading in editor (#2156)
34f6b3f4c Fix C++ RamseteCommand param doxygen (#2157)
f7a93713f Fix up templated TrapezoidProfile classes (#2151)
8c2ff94d7 Rename MathUtils to MathUtil for consistency with other util classes (#2155)
d003ec2dc Update to 2020v9 image (#2154)
8e7cc3fe7 Add user-friendly SimDeviceSim wrappers (#2150)
6c8f6cf47 Fix bug in cubic and quintic hermetic spline generation (#2139)
e37ecd33a Sim GUI: Add support for LED displays (#2138)
57c5523d6 Fix documentation in RamseteCommand (#2149)
7b9c6ebc2 Fix wpiutilJNIShared linkage typo in wpilibj (#2143)
9a515c80f Template C++ LinearFilter to work with unit types (#2142)
5b73c17f2 Remove encoder velocities methods in DifferentialDriveOdometry (#2147)
b8c102426 Fix PS3Eye VID and PID (#2146)
2622c6c29 Add default values for b and zeta in RamseteController (#2145)
f66ae5999 Add HSV helpers to AddressableLED (#2135)
5e97c81d8 Add MedianFilter class for moving-window median (#2136)
f79b7a058 Remove unnecessary constructor arg for LinearFilter's circular buffers (#2140)
e49494830 Replace Jenkins with Azure agent (#1914)
b67d049ac Check status of PDP CAN reads (#2126)
70102a60b SendableRegistry.foreachLiveWindow: Prevent concurrent modification (#2129)
6dcd2b0e2 Improve various subsystem APIs (#2130)
ce3973435 HAL_CAN_ReceiveMessage: return MessageNotFound if no callbacks registered (#2133)
3fcfc8ea7 Fix double disable segfaulting interrupts (#2132)
6ceafe3cd Fix class reference for logger (#2123)
b058dcf64 Catch exceptions generated by OpenCV in cscore JNI (#2118)
0b9307fdf Remove unused parts of .styleguide files (#2119)
39be812b2 Fix C++ ArmFeedforward (#2120)
21e957ee4 Add DifferentialDrive voltage constraint (#2075)
e0bc97f66 Add ProfiledPIDSubsystem example (#2076)
3df44c874 Remove Rotation2d.h wpi/math include (#2117)
a58dbec8a Add holonomic follower examples (#2052)
9a8067465 Fix incomplete .styleguide (#2113)
ffa4b907c Fix C++ floating point literal formatting (#2114)
3d1ca856b Fix missing typename and return type (#2115)
5f85457a9 Add usage reporting for AddressableLED (#2108)
4ebae1712 Enforce leading/trailing zeros in Java numeric constants (#2105)
fa85fbfc1 Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)
f62e23f1a Add Doxygen for new HAL interfaces (#2110)
5443fdabc Directly construct PWM port from HAL, avoid wpilib PWM object (#2106)
c0e36df9d Standardize on PWMVictorSPX in examples (#2104)
8c4d9f541 Add TrapezoidProfileSubsystem (#2077)
45201d15f Add encoder distance overload to DifferentialDriveOdometry (#2096)
845aba33f Make feedforward classes constexpr (#2103)
500c43fb8 Add examples for DMA, DutyCycle, DutyCycleEncoder and AddressableLED (#2100)
589162811 Use DifferentialDriveWheelSpeeds in RamseteCommand ctor (#2091)
b37b68daa Add JRE deployment to MyRobot Deploy (#2099)
0e83c65d2 Fix small logic error in ParallelDeadlineGroup (#2095)
6f6c6da9f Updates to addressable LED (#2098)
1894219ef Fix devmain package in commands (#2097)
77a9949bb Add AddressableLED simulation GUI support
a4c9e4ec2 Add AddressableLED simulation support
8ed205907 Add AddressableLED (#2092)
59507b12d Bump to 2020 v7 image (#2086)
5d39bf806 Make halsimgui::DrawLEDs() values parameter const (#2088)
841ef91c0 Use gyro angle instead of robot angle for odometry (#2081)
1b66ead49 Use standard constant for pi instead of 3.14 (#2084)
db2c3dddd Use DMA Global Number for DMA Index (#2085)
82b2170fe Add DMA support to HAL and WPILibC (#2080)
8280b7e3a Add DutyCycleEncoder and AnalogEncoder (#2040)
551096006 Use kNumSystems for DutyCycle count in Ports (#2083)
df1065218 Remove release configs of deploy in MyRobot (#2082)
bf5388393 Add deploy options to myRobot (#2079)
b7bc1ea74 Update to 2020v6 image (#2078)
708009cd2 Update to gradle 6.0 (#2074)
3cce61b89 Add SmartDashboard::GetEntry function in C++ (#2064)
565e1f3e7 Fix spelling in MecanumDriveOdometry docs (#2072)
1853f7b6b Add timing window to simulation GUI
c5a049712 Add simulation pause/resume/step support
f5446c740 Add Notifier HALSIM access
3e049e02f Add name to HAL Notifier
2da64d15f Make usage reporting enums type match (#2069)
f04d95e50 Make FRCUsageReporting.h C-compatible (#2070)
d748c67a5 Generate docs for command libraries and fix doclint enable (#2071)
55a7f2b4a Add template for old command-based style (#2031)
486fa9c69 Add XboxController examples for arcade and tank drive (#2058)
e3dd1c5d7 Fix small bug in SplineHelper (#2061)
7dc7c71b5 Add feedforward components (#2045)
5f33d6af1 Fix ProfiledPIDSubsystem parameter name (#2017)
94843adb8 Standardize documentation of Speed Controllers bounds (#2043)
9bcff37b9 Add HAL specific version of wpi_setError (#2055)
326aecc9a Add error message for CAN Write Overrun (#2062)
00228678d Add requirements param to more Command APIs (#2059)
ff39a96ce Make DigitalOutput a DigitalSource (#2054)
5ccad2e8a Fix frc2::Timer returning incorrect timestamp values (#2057)
629e95776 Add VendorDeps JSON files for command libraries (#2048)
6858a57f7 Make notifier command tests a lot more lenient (#2056)
0ebe32823 Fix MyRobotStatic accidentally linking to shared command libs (#2046)
384d00f9e Fix various duty cycle bugs (#2047)
1f6850adf Add CAN Manufacturer for Studica (#2050)
7508aada9 Add ability to end startCompetition() main loop (#2032)
f5b4be16d Old C++ Command: Make GetName et al public (#2042)
e6f5c93ab Clean up new C++ commands (#2027)
39f46ceab Don't allow rising and falling values to be read from AnalogTrigger (#2039)
d93aa2b6b Add missing lock in FRCDriverStation (#2034)
114ddaf81 Fix duplicate encoders in examples (#2033)
f22d0961e Sim GUI: Add duty cycle support
3262c2bad Sim GUI: Use new multi-channel PDP getter function
96d40192a Revert accidental change to MyRobot.cpp (#2029)
ed30d5d40 Add JSON support for Trajectories (#2025)
2b6811edd Fix null pointer dereference in C++ CommandScheduler (#2023)
1d695a166 Add FPGA Duty Cycle support (#1987)
509819d83 Split the two command implementations into separate libraries (#2012)
2ad15cae1 Add multi PDP getter and sim PCM/PDP multi arg functions (#2014)
931b8ceef Add new usage reporting types from 2020v5 (#2026)
0b3821eba Change files with CRLF line endings to LF (#2022)
6f159d142 Add way to atomically check for new data, and wait otherwise (#2015)
a769f1f22 Fix bug in RamseteCommand (using degrees instead of radians) (#2020)
c5186d815 Clean up PIDCommand (#2010)
9ebd23d61 Add setVoltage method to SpeedController (#1997)
f6e311ef8 Fix SplineHelper bug (#2018)
f33bd9f05 Fix NPE in RamseteCommand (#2019)
1c1e0c9a6 Add HAL_SetAllSolenoids to sim (#2004)
ea9bb651a Remove accidental OpenCV link from wpilibc shared library (#2013)
cc0742518 Change command decorators to return implementation (#2007)
16b34cce2 Remove IterativeRobot templates (#2011)
669127e49 Update intellisense to work with Beta 2020 code (#2008)
9dc30797e Fix usage reporting indices (#2009)
f6b844ea3 Move HAL Interrupt struct to anonymous namespace (#2003)
a72f80991 Add extern C to DigitalGlitchFilterJNI (#2002)
916596cb0 Fix invalid examples json, add validator (#2001)
5509a8e96 Use constexpr for all example constants
0be6b6475 Use constexpr for DifferentialDriveKinematics
936627bd9 wpilibc: Remove direct CameraServer dependency (#1989)
8e333c0aa Use FPGA Time instead of wall clock time for odometry (#1996)
d4430b765 Gearsbot example: Use standard argument order (#1995)
75438ab2c Add RamseteCommand (#1951)
989df1b46 Bump Native Utils and OpenCV dependencies (#1993)
dbc33b61e Fix Timer usage in TrapezoidProfileCommand (#1992)
79f8c5644 Add TrapezoidProfileCommand (#1962)
9440edf2b Refactor TrajectoryGenerator (#1972)
73a30182c Add frc2::Timer (#1968)
36ea865ed Add toString for geometry and trajectory classes (#1991)
cbe05e7e8 Update ProfiledPIDController API (#1967)
d04eb3546 Deprecate old PID classes (#1964)
02264db69 Add JNI dependencies to myRobotCpp (#1980)
2a76c996e Use VID/PID detection for PS3Eye (#1977)
a3820bbdf Remove HAL_BaseInitialize (#1981)
a83fb4793 Update to 2020v5 image (#1983)
4b0ed910e Make SwerveDriveKinematics.toChassisSpeeds() public (#1976)
103c1b121 Remove DS caching from the HAL level (#1971)
6635ea75e Fix NPE in SendableRegistry.foreachLiveWindow() (#1974)
cfe23c5cd Fix grammar error in comment for configureButtonBindings (#1969)
4bde2654e Fix mac azure build (#1973)
4f034e6c1 generateTrajectory: default reversed param to false (#1953)
acf960f72 Sim GUI: Add option to disable outputs on DS disable
2d3dac99f Sim GUI: Handle low resolutions and scale default window positions
07c86e0cd Sim GUI: Support High DPI monitors
46ad95512 SimDeviceData: Add missing null check
5bce489b9 Add ProggyDotted font to imgui (both cmake and gradle)
55af553ac Simulation GUI: Map gamepad the same way as DS
c59f9cea5 CameraServer: Add VID/PID support for Linux USB devices (#1960)
3fc89c84d Make splinePointsFromSplines public (#1963)
2c5093797 Fix implicitly deleted move constructors (#1954)
f3ad927f4 Update Java SmartDashboard and LiveWindow to match C++
05c25deb7 Fix move handling of C++ Sendable in SmartDashboard and LiveWindow
d726591ce Fix Gazebo sim plugin build (#1959)
2ff694fa4 Unbreak gradle build when other compilers installed (#1958)
53816155b Improve command decorator names (#1945)
a38f183a9 Fix GenResources.cmake so it's usable in a submodule (#1956)
b3398dca3 Set gradlebase correctly for all examples (#1950)
2c311013d Add Aarch64Bionic platform detection (#1922)
c10f2003c Add generateTrajectory overload (#1944)
63cfa64fb Add getters for pose in odometry classes (#1943)
2402c2bad Fix C++ command group recursive constructor bug (#1941)
f4eedf597 Fix ConcurrentModificationException in CommandScheduler (#1938)
bb0b207d2 Fix array out of bounds exception caused by parallel race group (#1935)
7bd69e591 Fix typo in temperature (#1940)
ec9738245 Bump to 2020 v4 image (#1931)
46303a822 Add messaging to extension loading in the HAL (#1926)
d169d6be9 Set extract_static for Doxygen config so that static members show up (#1930)
4e183eb10 Bump to 2020 v3 image (#1929)
84c185803 LiveWindow: catch errors in callback/builder functions (#1921)
0e3b0f3da Remove deprecated free() calls (#1925)
7f839b87c Remove timeouts from azure builds (#1924)
45b766a5d Fix main thread ID being potentially incorrect for simulation (#1923)
56d782b16 Add secondary camera name for PS3Eye (#1920)
2b4894038 Add simulation GUI plugin
f97d16073 Add imgui build to cmake
55a844a3e HAL sim: Add encoder channel B access
10deba854 Remove sendables from SendableRegistry when close() is called (#1917)
a9f0e4668 Implement sim devices for ADXL345, ADXL362, ADXRS450, Ultrasonic
aa9064586 Add ability to associate other devices with a SimDevice
81c2c8a7d Add simulation generic device/value support
e8d6f8a2c Move mockdata/HAL_Value.h to hal/Value.h
1b266717a Add simulation module support to cmake build (#1906)
fb8f3bd06 Add testbench yaml file (#1915)
846d8def0 Update to 2020 v2 image (#1913)
d6ac6e512 Fix PortForwarder package declaration (#1912)
227157086 Fix PS3Eye exposure setting (#1911)
885744d7e Add myRobot C++ version to cmake build (#1907)
366091fa8 Document that ConditionalCommand requires all subsystems (#1909)
c58b072c8 Fix Drive usage reporting order (#1908)
762c88adb Update compiler versions in readme (#1905)
af8ce568d Add Ramsete unicycle controller (#1790)
b2c2934d0 Fix javadoc warnings about invalid usage of ">" (#1904)
cce26ec78 Replace CRLF line endings with LF (#1902)
cb54602d4 Add support for writing RTR CAN Frames to the CAN API (#1900)
9f740e590 Use OS for serial port instead of NI VISA (#1875)
b23baf611 Add ability to run robot main loop in a separate thread (#1895)
457f94ba2 Add trajectory generation using hermite splines (#1843)
fd612052f Update native utils to use new frchome directory (#1884)
8858ec55c Remove periodic can read call (#1868)
41efb8015 Update CANAPITypes.h (#1860)
c93be1b2d Remove LabVIEW HAL support (#1901)
680f8919e Remove eigen, units and libuv from doxygen generation (#1898)
c5812524f Bump GradleJNI plugin version (#1899)
971303da8 Add PortForwarder class (#1890)
50db77bf2 Fix wpiutil cmake eigen install source directory (#1891)
85d42c199 C++ PIDCommand: Add GetMeasurement() and UseOutput() (#1892)
2dfbb855d wpilibj: Fix SwerveDriveKinematics ctor parameter name (#1889)
471f375a3 Simplify Sendable interface (#1864)
1d8c4d016 Replace ::value and ::type with _v and _t suffixes (#1885)
a5650b943 Add Units Utility class for Java (#1829)
904479ad4 Deprecate GearTooth class for removal (#1878)
86b666bba Add equality comparator to geometry classes (#1882)
62f07c182 Make one-arg Rotation2d constructor implicit (#1883)
f405582f8 Add kinematics suite (#1787)
561cbbd14 Deprecate Filter class for removal (#1876)
84e2973aa Remove unused include from Filesystem.h (#1877)
f49859ebf Remove NI VISA headers, as they are now included in NI Libraries (#1879)
bc59db5e6 Rename DEBUG macro to DEBUG0 (#1871)
dd928b4cb Remove JNI logging (#1872)
3e0f7d099 Use units for new NotifierCommand (#1869)
5ffe15d5f Remove ability to build all combined artifacts (#1867)
516cbef2c  Remove RoboRIO ifdef from simulation headers (#1859)
9b6ffc201 Replace SetOutputRange() with SetIntegratorRange()
ff8b8f0a8 Remove percent tolerance from PID controller
0ca8d667d Clean up AutoCloseable and other Java warnings (#1866)
7112add67 Watchdog: use units::second_t instead of double (#1863)
761bc3ef8 Change C++ WaitCommand to use units (#1865)
1fb301123 Add MathUtils.clamp() for Java (#1861)
eb3e0c9c9 Fix cmake Eigen include directory (#1862)
2250b7fbe Rename GearsBotNew example to GearsBot
c9f9feff1 Replace deprecated API usage in C++ examples
d6b9c7e14 CONTRIBUTING.md: Point to frc-docs instead of screensteps (#1858)
d10a1a797 Fix eigen build in vcpkg (#1856)
2bdb44325 Add frc2 includes to list of "other lib" regexes (#1855)
4b2b21d24 Replace outdated Java collections (#508)
8993ce5bf Move Eigen headers out of main include folder (#1854)
0f532a117 Add PWMSparkMax (#1751)
f7ad363d8 Add jni cross compile options for aarch64 (#1853)
9afea3340 Add support for aarch64 jetson bionic builds (#1844)
d787b5d60 Add more items to .gitignore (#1850)
5dd0d1b7d Use units in SPI
07ac711b3 Fix units deprecated warning in IterativeRobot
decfd858b Correctly report -1 for POV on disconnected joystick (#1852)
076ed7770 Add new C++ Command framework (#1785)
a0be07c37 Refactor HAL handle move construction/assignment (#1845)
558c38308 Add new Java Command framework (#1682)
1379735af Delete RobotState and SensorUtil constructors (#1847)
e3d86fee4 Move circular buffer class from wpilib to wpiutil (#1840)
4cd8a5667 TimedRobot.cpp: Fix deprecation warning (#1846)
b2861f894 Use 2020 artifacts and artifactory server (#1838)
98cc32703 Update to use artifactory to publish artifacts (#1833)
fa0640300 Move drive integration tests into wpilibj/src/test (#1836)
e716c36b8 Fix Nat.java generation to be incremental (#1831)
9fd2b5e3f Fix MSVC builds on cmake windows in vcpkg (#1835)
7e95010a2 Add compile-time EJML matrix wrapper to wpiutil (#1804)
3ebc5a6d3 Add ProfiledPIDController
fc98a79db Clean up PIDController interface in preparation for ProfiledPIDController
fdc098267 Fix compilation error in elevator trapezoid profile example (#1826)
a3dd84e85 Make XBoxController Button enum public (#1823)
a216b9e9e Add TrapezoidProfile example (#1814)
8f386f6bb wpilibc: Add unit-safety to C++ geometry classes (#1811)
c07ac2353 wpilibc: Add overloads for units (#1815)
f1d71da8a Move GetStackTrace and Demangle to wpiutil, add Windows support (#1819)
ef037457e Make LinearFilter copyable and moveable (#1789)
76930250c Remove objective-cpp support (#1816)
1c246418f Move TrapezoidProfileTest to trajectory folder (#1812)
95a54a0f2 Add java arcade drive example (#1810)
a4530243e HAL sim: Fix incorrectly setting dio port to initialized on cleanup (#1813)
09d00a622 Update Java examples to use new PIDController (#1809)
ba9b51742 Add missing Java examples (#841)
6411bd79c InterruptableSensorBase: Fix callback function deletion (#1807)
810e58ea8 I2C: Add tip about writeBulk() to transaction() (#1806)
607d6c148 Fix wpilibj integration tests jar name (#1808)
c9873e81b Remove PIDControllerRunner and mutex from new PIDController (#1795)
98d0706de Fix cscore build with OpenCV 4 (#1803)
fbe67c90c Make Sendable setters synchronous (#1799)
c67a488a0 Format SendableBuilderImpl javadocs (#1802)
8e93ce892 Fix PIDControllerRunner member destruction order (#1801)
c98ca7310 Add EJML dependency to wpiutil (#1769)
3b12276bc SendableBase: remove unnecessary synchronization (#1797)
e6d348f38 Fix missing default name in Java PIDController (#1792)
df12fc2a8 Java cleanups (#1776)
39561751f Update GradleVSCode version (#1786)
37d316aa0 Add C++20 std::math constants shim (#1788)
dd4310959 Deprecate frc/WPILib.h (#1779)
823174f30 Update native utils to 2020.0.4 (#1783)
37c695266 Squelch -Wdeprecated-copy for Eigen with GCC >= 9
04c9b000f Revert "Fix build of Eigen 3.3.7 with GCC 9"
ca3e71e21 wpiutil: Fix Process::Spawn() (#1778)
d946d5a2b Fix Eigen compilation errors and add tests (#1777)
8b1b9ac75 Fix build of Eigen 3.3.7 with GCC 9
2f680ba99 Add Eigen linear algebra library
a885db7d4 Make MotorEncoderTest use LinearFilter (#1775)
ee2410169 Add geometry classes (#1766)
48fe54271 Add HALSIM_SetSendError implementation (#1773)
dff58c87f Fix unused warning in release build (#1771)
dde61aad3 Remove TimerEventHandler typedef from Notifier class (#1767)
0f6ef80ab Add RobotState#IsEStopped and DriverStation#IsEStopped (#952)
e48886187 Move unit tests from integration test suite (#1170)
dffa1a5cb Make null checks more descriptive (#1688)
fe59d854d Notifier: add null check (#1684)
10731f3d6 Update uv Udp wrapper for latest features
89f7b72b6 Update libuv to 1.30.1 release
85f2f8740 wpiutil: Add unique_function (#1761)
73ec94078 Remove SampleRobot (#1658)
62be0392b Replace std::lock_guard and std::lock with std::scoped_lock (#1758)
24d31df55 Make sure move constructor is generated for TrapezoidProfile (#1757)
841ef5d73 Remove template types from lock RAII wrapper usages (#1756)
e582518ba Fix some move constructors (#1754)
8757bc471 Remove pre-C++17 shims (#1752)
ea9512977 Add replacement PIDController class (#1300)
9b798d228 Add TrapezoidProfile class (#1673)
804926fb5 Unconditionally skip athena builds for sim (#1748)
118e9d29d Add C++14 units library (#1749)
c705953d7 Add usage reporting to LinearFilter (#1750)
852d1b9ca Don't cross-build gazebo for raspbian (#1747)
eedb3a1ad Fix GCC 9 warnings (#1730)
60dce66a4 Remove wpi::ArrayRef std::initializer_list constructor (#1745)
9e19b29c3 Use base azure image for primary wpilib build (#1744)
299425071 Update jni library, fix cross builds of the jni symbol check (#1742)
a6b0e9b85 Only disable execution of cross compile google tests (#1741)
3c2093119 Use docker container to run wpiformat (#1740)
5fe2eebce Revert "Don't build halsim_gazebo on raspbian (#1737)" (#1743)
4b1b92bb7 Replace wpi::optional with C++17 std::optional (#1732)
0fbb0d989 Update to 2020 compilers (#1733)
2dc94e605 Disable google tests on cross compilers (#1738)
d9cb57a42 Don't build halsim_gazebo on raspbian (#1737)
f7cfdd7ce Replace crlf line endings with lf (#1731)
b6d5d90d9 Add JaCoCo Support (#1734)
c7ab2baa6 Add way to disable the jni check tasks from a property (#1736)
0c45c5b7e Fix skip athena and skip raspbian flags (#1735)
3dfb01d45 Update to new Native Utils (#1696)
30e936837 Clean up LinearDigitalFilter class (#782)
311e2de4c Remove deprecated Joystick constants (#1715)
c08fd6682 Update CAN manufacturer list (#1706)
258bba0c2 ErrorBase and WPIError improvements (#1727)
372ca4f45 cmake: Enable googletest unit tests (#1720)
223d47af0 HALSIM: support mocking of HAL_SendError() (#1728)
55cb683db Change compiler flags to C++17 (#1723)
ee8a33c56 wpiutil: SafeThread: Add thread id, support getting shared_ptr (#1722)
61426d08d wpiutil: Signal: make operator() const (#1721)
b630b63ef Remove functions in LiveWindow deprecated since 2018 (#1716)
1d0c05d4f Styleguide fixes for #1718 (#1719)
f07569df1 Fix newer GCC/clang compiler warnings (#1718)
0120f3124 C++ SPI: Fix SetClockRate to take int (#1717)
c2829ed98 Configure gradle to ignore unresolved headers (#1711)
221e66f46 Allow disabling static init of JNI libraries (#1672)
738852e11 cmake: Add cross toolchain files for Rio and Pi (#1710)
27b697b08 Remove frc directory include shims (#1714)
9e45373a7 Remove functions and classes deprecated for 2018 season (#1059)
eeb1025ac SPI: Report port as instance for usage reporting (#1704)
bc6f1e246 Windows compiler options improvements (#1709)
bb48ae391 cmake: Move example programs into folders (#1654)
221011494 Update for C++17 and fix MSVC warnings (#1694)
fb1239a2a Add raw sources and sinks to cscore (#1670)
7de947734 Add lambda overloads for interrupts (#1636)
90957aeea Move libuv to its own subfolder in build (#1661)
47aae502a Styleguide fixes (#1702)
0bff98b5e Correct DifferentialDrive::ArcadeDrive param docs (#1698)
b52e40b80 Allow widgets to be added by passing value suppliers (#1690)
4a00cd77b Add usage reporting for the Shuffleboard API (#1685)
e25e515f2  Publish artifacts on azure (#1678)
322ef9b96 Force Java 11, fix javadoc generation (#1695)
d42ef5df0 Fix Watchdog print formatting (#1693)
f432f65be Update copyright year in license to 2019 (#1524)
1726b77ac wpiutil: uv: Remove copy from SimpleBufferPool (#1680)
620bec9ca wpiutil: uv: Add LoopClosing status to Handle (#1647)
7cd6e2e7f UsbCamera: Solve race in windows initialization (#1638)
7732836bd Completely disable watchdog tests on mac (#1679)
698edfda9 Remove framework load, disable mac timeout test (#1676)
1c454b000 Add Shuffleboard calls to IterativeRobotBase in C++ (#1607)
f42905b32 Include missing headers in HAL.h (#1660)
bdc822fad Only generate passthrough URLs for RoboRIO (#1624)
d3affb16b Make failure of HAL_GetFPGATime() more descriptive (#1633)
2de3bf7f5 Update LLVM from stable upstream (#1653)
3cf4f38f5 Fix build on macos10.14.4 (#1648)
4e0c10f48 Fix CAN Clean using wrong ID (#1668)
3b0631324 Fix Gray to BGR conversion in CameraServer (#1665)
6cd1c73ef Fix GUID comparison creating weird symbol (#1659)
063bbab6f MavenArtifacts.md: update links to HTTPS (#1674)
aab4c494d Fix type in build.gradle (#1604)
bf46af260 Disable extraneous data warnings in libjpeg (#1630)
655763a9a Limit length of message sent to DS SendError call (#1618)
a095ec2d8 Fix linker errors with free functions in Threads.h (#1625)
12ab035aa Fix receive side of LabVIEW USB streams (#1621)
99e4f7dd2 Fix SPI CS1 not working correctly (#1614)
60c2f5905 C++ CameraServer: initialize default Usb camera device number (#1601)
d55ca191b CameraServer: Add switched camera support (#1600)
e8b24717c C++ Shuffleboard fixes (#1595)
182758c05 Fix Ultrasonic sensor runner thread (#1598)
74f7ba04b Java PIDBase: Make setPIDSourceType and getPIDSourceType public (#1599)
997d4fdf4 Update HAL GetStackTrace to properly report user errors (#1594)
76d9e2663 uv: Add reuse to pipe (#1577)
a230c814c Add support for WPILib vscode extension for allwpilib C++ intellisense (#1590)
12cb77cd7 Fix DS Comm button tests (#1591)
8a9822a96 Allow multiple instances of the same PDP (#1582)
a9371a758 Fix missing exposure property on windows USB cameras (#1571)
6992f5421 cscore: Avoid crash if invalid (null) source set in MjpegServer (#1585)
43696956d Fix Watchdog incorrectly resetting expiration flag upon disable (#1580)
ae3fd5ada Fix docs search having unspecified module directory (#1568)
404666b29 Fix Halsim DS Button Format (#1583)
1eb4c99d1 Update README for 2019 changes (#1569)
910b9f3af Add support for camera descriptions on windows (#1572)
09d90b02f Remove prints and unecessary items from windows GetNetworkInterfaces (#1573)
0e1f9c2ed ntcore: Read ini escaped quotes correctly (#1579)
f156a0011 wpiutil uv: Pass IPC value of listening pipe to accepted pipe (#1576)
4a6087ed5 Disable watchdog test on mac (#1578)
88a09dd13 cscore: Handle USB camera integer menus (#1561)
7d1959636 Changed terminology from "Overload" to "Override" (#1563)
bd05dfa1c Fix ds_socket matchinfo (#1555)
05d6660a6 cscore: Add config json to VideoSink (#1543)
1349dd4bd Make MjpegServer functions public (#1545)
fdf298b17 CameraServer: Return MjpegServer from StartAutomaticCapture(VideoSource) (#1544)
453a9047e Fix cmake hal sim build (#1539)
e97e7a761 Add DriverStationSim.notifyNewData (#1537)
308bdbe29 cscore: Windows UsbCamera: Use custom windows message for initial connection (#1534)
f889b45d5 cscore: MjpegServer: Fix FPS limiting (#1536)
444b899a9 Java: Fix Timer.get() handling of accumulated time (#1531)
f121ccff0 Avoid Watchdog thread clobbering m_isExpired flag after callback (#1527)
bc2c932f9 Fix MotorSafety deadlock (#1526)
6bdd7ce50 Update docs for disabled init to match all the other modes (#1523)
c12d7729e Update examples to use VictorSPX's rather then Sparks (#1521)
363511604 cscore: set charset of displayed pages (#1520)
6105873cb Provide alternate device paths info for USB cameras (#1519)
80f87ff8a Allow video sources to be added to Shuffleboard (#1453)
a2368a619 Watchdog: add timeout message suppression feature
ae3cb6c83 Watchdog.java: add comment fixes from reverted #1442
f0f196e5b Revert "MotorSafety: Use Watchdog instead of DS class polling (#1442)"
7c35355d2 Revert "Suppress Watchdog's generic timeout message in MotorSafety (#1486)"
75cc09a9e Revert "Suppress timeout message in C++ MotorSafety class (#1512)"
0e2e18063 PowerDistributionPanelTest.cpp: Include thread
01d132206 Add constants for built-in Shuffleboard widgets and layouts (#1468)
ceed1d74d ntcore: Strip whitespace from IP addresses (#1516)
e1bf62399 cscore: handle HTTP stream hangs (#1513)
d46ce13ff Fix CAN API timing out incorrectly (#1497)
300eeb330 Suppress timeout message in C++ MotorSafety class (#1512)
d81700125 Only write version information on real robot (#1510)
8ac4b113a Fix build on macOS 10.14.2 (#1509)
f3864e9ab Move deprecated message in C++ from class level to constructor (#1505)
799c3ea8a Disable gyro sim test on mac (#1506)
8d95c38e3 Watchdog: Fix potential IllegalFormatException (#1508)
a7f4e29b7 Update versioning and vscode plugin (#1498)
b88369f5e Move deprecation warning for SampleRobot to constructor (#1503)
ce6f1d058 Change deperecation year for serial port direct constructor (#1504)
f163216a4 Add support for raspbian extraction (#1494)
c449ef106 Unconditionally await in awaitUntil (#1499)
6593f4346 Default to DS attached to true in sim (#1500)
ce1367a11 Make JNI library not found message clearer (#1495)
0d7d88026 Renable full Java 8 Compatibility (#1493)
ca2acec88 Update to NI Libraries 2019v12 (#1492)
3721463eb Don't use symbolic path searching for USB detection (#1491)
6e8f8be37 Make C++ examples able to run GradleRIO unit tests (#1490)
d84240d8e Bump opencv version to 3.4.4-4 (#1489)
1823cb2b6 Fix periodic can packets not being received properly (#1488)
41596608c Suppress Watchdog's generic timeout message in MotorSafety (#1486)
0c3b488e1 Make ntcore instances not smart pointers (#1485)
7d7af287f Add empty classes to command/classes creation (#1479)
411962299 Fix characters in license file (#1480)
d528a7796 wpiutil: Fix resources build (#1476)
dab7e1b7a Add multiCameraServer to cmake build (#1477)
ab4934546 Add missing param javadoc comment and fix argument order inconsistency (#1475)
608d82423 wpiutil: Use scoped connections to ensure cleanup (#1474)
e0e15eafe wpiutil: Add shared resources for http servers
0fb24538a wpiutil: HttpServerConnection: add SendStaticResponse
d65547ea7 Bump OpenCV version (#1471)
bfe15245a Add multiCameraServer executable (#1422)
ff58c5156 Change SmartDashboard type of PIDBase.cpp to match PIDBase.java (#1470)
6d4326a56 Rate-limit Watchdog timeout prints to 1Hz (#1459)
97ba195b8 Fix LiveWindow attempting to start listeners on uninitialized sendables (#1463)
3d546428a Add missing SubTableListener methods to C++ NetworkTable (#1465)
b64dfacff DriverStation: fix error checking for GetXXXAxis and GetXXXPov (#1469)
dcbf02a1e Update auto SPI for timestamp changes (#1457)
7e1ec2883 NI image 2019v9 (#1467)
ef16317f8 Update NativeUtils and Doxygen (#1462)
26e8e587f MotorSafety: Use Watchdog instead of DS class polling (#1442)
0d0492bfc HAL: Add additional error messages to HAL_GetErrorMessage (#965)
3b33abfc7 Make Watchdog use single thread dispatch (#1347)
99033481e Add netconsoleTee
b4901985b Add netconsoleServer
97edb6c68 wpiutil: uv::Buffer: Add Dup() function
73de3364b AxisCamera constructor: add StringRef overload (#1458)
5551981b3 Upgrade to Gradle 5.0 (#1444)
90572a3cc Add wpilibc and wpilibj skeleton templates (#1441)
c40518805 Fix directory iterators not working correctly (#1454)
bea0565ac Update to OpenCV 3.4.4 (#1452)
0b0345436 wpiutil: Replace LLVM Optional with C++17-compatible optional
489701cac wpiutil: Rename Optional.h to LLVMOptional.h
a769d56ec Create C++ ShuffleBoard example (#1438)
6f0c185a0 Add methods to change the selected tab in the Shuffleboard app (#1448)
a60f312d1 Add eager null checks in drive base classes (#1447)
acb786a79 Remove MotorSafetyHelper, create MotorSafety base class instead (#562)
df347e3d8 wpiutil: uv::Process: Revamp args/options approach (#1434)
e4aa45f34 wpiutil: WebSocket: Fix Sec-Websocket-Accept computation
75cc3cda2 wpiutil: SHA1: Add method to get raw bytes instead of hex
45f4472d4 Add mechanism to control Shuffleboard recordings and add event markers (#1414)
69cb53b51 Add support for USB Webcams on Windows (#1390)
70a66fc94 Make Gradle show stdout for wpilibj tests (#1431)
9207d788a Convert ReadInterrupt* to return int64_t time (#1417)
ef3a31aa2 Add an example of using the HAL directly (#1374)
63775362f Remove SynchronousPID class (#1429)
55493b0c1 Fix polymorphic exception types being caught by value (#1426)
1696557c4 Fix deprecation warning in wpilibj shuffleboard example (#1427)
ecd376be4 Export C functions from JNI statically linked library (#1425)
f54c0f70f Update opencv to 3.4.3-19 (#1424)
9bc998f4b cscore: Add JSON for source settings (#1423)
43d188a42 RobotBase: Call cscore Shutdown() from destructor
563d5334c Update OpenCV to 3.4.3-17
193b0a222 cscore: Add Shutdown() function
76f5d153f wpiutil: Add Stop() to EventLoopRunner
19caeca99 cscore: Use more standard naming for Instance public variables
0abae1765 cscore: Refactor sink and source creation
81d10bc65 cscore: Build examples on Windows (#1421)
b51b86525 Stop webserver on test run (#1420)
ace37c517 CameraServer: enable usb cameras on non-Linux platforms (#1411)
ac751d322 Fix unused variable warnings in ntcore (#1416)
7c9a3c4d7 Update NI Library to 2019.7.1 and OpenCV to 3.4.3-16 (#1418)
8be693f55 Fix list layout in shuffleboard example (#1413)
622ae29df CameraServer: Change opencv sources to publish "cv:" type (#1412)
e7c98feca libuv: Use WPI (FPGA) clock on roboRio (#1409)
28087424e Add deploy directory detection (#1400)
b6830638d NetworkListener: use Pimpl idiom (#1405)
fb557f49e Add missing driver station documentation (#1388)
746f950a0 Remove ifdefs from linux NetworkListener (#1404)
9a38a3e18 Don't use static for raw_ostream outs/errs (#1401)
2e3e3a47b Use a kill -9 after attemting a safe kill (#1397)
e27d6d7bb cscore: Change impl to only one singleton (#1398)
1dec0393a Fix static deletion race condition in DS thread (#1396)
d03b02032 wpiutil: Add WorkerThread (#1302)
71e29b1d9 Remove unused import from rumble example (#1395)
f0b0965f9 Remove large HAL headers from wpilibc headers (#1386)
f774e47c8 Add an example showing how to use a hid rumbler (#1394)
761933a16 Refactor Java Ultrasonic to use a List container (#1389)
99e0f08a6 Move applicable integration tests to native build as unit tests (#1364)
e89d5eb69 Fix stringop truncation warning from GCC 8.2 (#1393)
2501e1188 Enable HTML5 javadocs (#1267)
9174f23f3 Remove some usages of windows.h (#1370)
9f6544fa8 Allow binding commands to POV (#1378)
9a1af132b Unify GetHostname() and use libuv implementation (#1391)
a8aacd365 Update build setup for raspbian and debug binaries (#1384)
8ff81f5a2 cscore: Separate platform-specific sources (#1387)
349e273ec AnalogGyro: add "calibrating for n seconds" message (#1380)
0a2ab4f0d Revert change in behavior in HeldButtonScheduler (#1381)
7c1a7332e uv::Async, uv::AsyncFunction: allow calling from within loop
172e438cd wpiutil: uv::Async: Keep weak reference to loop
1a7a0db1f wpiutil: Change uv::AsyncFunction to use promise/future.
11e5faf46 Use Array Constructor rather then new array to toArray (#1368)
c7118f8ad wpiutil: Signal: Don't use std::forward when calling (#1371)
7933d2cbe wpiutil: uv: Don't close uninitialized handles (#1372)
ce8c71b1f Fix link for license (#1367)
da9a57552 Rename squaredInputs to squareInputs in DifferentialDrive (#1361)
7068551a3 Bump OpenCV and GTest to 2019 dependencies (#1366)
bd9484a2f Make MyRobot compilation test use TimedRobot (#1363)
b9fa3a439 Update to 2019 RoboRIO and 2019 v4 image (#1352)
88b93c220 Update NativeUtils to not copy NI libraries to jenkins RoboRIO (#1359)
0a937bb5b wpiutil: SafeThread: Ensure thread is released in destructor (#1358)
613d5eda0 wpiutil: SafeThread: join on thread exit (#1357)
18c8cce6a SafeThread: Avoid use-after-free risk in thread shutdown (#1355)
36000ddb3 wpiutil: uv::Loop: Store the thread ID of the loop
de6d6c9a5 wpiutil: EventLoopRunner: Allow getting the loop shared_ptr.
6d99c0ac6 wpiutil: EventLoopRunner: Remove extraneous wpi namespace qualifier
164e9a2c7 wpiutil: uv::Work: Don't connect work and afterWork if they're null
f3fb95af7 wpiutil: uv::Tcp: Simplify reuse function
40a9fc44f wpiutil: uv::Poll: add reuse functionality
f0ac04864 Remove pmd and checkstyle publishing from azure (#1353)
81498e6af Deprecate IterativeRobot in favor of TimedRobot (#1341)
f1056efa0 Updates version plugin to 2.2 (#1349)
54fbec27d Fix typo (#1348)
fd8215345 Fix shuffleboard C++ tests, and run them on desktop (#1351)
7b471d8c6 Fix windows image on azure pipelines (#1350)
175c6c1f0 Add fluent builders for more flexibly adding data to Shuffleboard (#1022)
ac7dfa504 Switch to using containers for Linux builds on Azure (#1335)
a73285486 Clean up edge detection logic in ButtonScheduler subclasses (#1340)
617185602 Document Watchdog epochs better (#1345)
d5d744a39 Fix publishing of templates on jenkins (#1343)
8b1274d74 Replace typedefs in C++ with using declarations (#1339)
26c33a9a5 Remove priority_condition_variable (#1337)
5fad2b105 Remove travis and appveyor configuration files (#1338)
32ec07ee0 Throw correct exception in HAL_getJoystickAxes/POVs JNI (#1336)
15c5a820b Publish tests to azure ci (#1334)
e15fabd2e Add cmake azure build (#1332)
1aa844672 Add move constructors and assignment operators to wpilibc (#1314)
b1965f74a Add styleguide check to azure pipelines build (#1331)
0c58a0a70 Repackage CameraServer classes (#1321)
467c9fd68 Add kInvalid value to HAL_I2CPort and HAL_SPIPort (#1329)
b505bbefd Rename variable in SPI class not compliant with style guide (#1330)
5c6b78ea2 Set up CI with Azure Pipelines (#1306)
f89274fb1 Fix hal header zip task dependencies (#1327)
1137582a7 Revert "Move deprecated ntcore classes to wpilibj jar (#1322)" (#1326)
e26e3b6aa Fix HAL Headers Zip (#1325)
456d3e16a Update NI Libraries to layout without C++ (#1324)
e21007304 Move HAL classes to their own base package (#1317)
0068b6aea Remove wpilibj tests from wpilibjIntegrationTests (#1323)
12c92a822 Move deprecated ntcore classes to wpilibj jar (#1322)
d2a5aaafd Use external dependencies for NI and NetComm libraries (#1304)
bedef476f Replace IterativeRobot in examples with TimedRobot (#1310)
59386635e Add CAN API constructor that takes explicit manufacturer and device type (#1311)
a846ed062 Add virtual destructor to CameraServerShared (#1313)
8b5dc53cc Add Lambda support to InstantCommand (#1262)
59700882f PIDController: Mention unit for 'period' (#1305)
025af2452 Make NetworkTableEntry.callRpc and createRpc public (#1303)
c0ff6198b Change hal sim to use spinlocks (#1291)
67b1c8531 Notifier: properly reset HAL alarm in non-periodic case (#1296)
0b113ad9c Fix some PIDCommand constructors not forwarding subsystems (#1299)
c8482cd6d wpiutil: Add WebSocket implementation (#1186)
d6d532182 wpiutil UidVector: Implement clear() and forward iterator (#1293)
8d91343bf wpiutil spinlock.h: Include STL mutex for convenience (#1292)
488ba7937 Add more checkstyle checks to simulator code (#1289)
de212a9dd Add names to simulator components (#1268)
8d8f120cc EventLoopRunner: Use AsyncFunction
57490e000 Add uv::AsyncFunction to do a roundtrip function call to a uv::Async
1de1900db Change uv::Async to accept data parameters
4a3e43d4a Add HttpMultipartScanner (#1197)
9e37ee13d Add wpi::HttpServerConnection and an example of its use (#1281)
7b95c5341 ntcore: Change params in Java RpcAnswer from String to byte[] (#1280)
5283726cc ntcore: Describe RPC version 0 in spec (#1279)
0b8f4b5e6 Add libdl to wpiutil cmake build (#1288)
a5f7342fc Run cmake build in travis (#1287)
6df742544 cmake: Compile cscore and wpiutil examples (#1286)
d7b68f3f9 Unbreak cmake build (#1285)
e28295fc7 Add dependency injection of Subsystem to Command (#1275)
6df500e72 Add missing library dependencies in simulation dev executables (#1284)
83cfb8b19 UrlParser: fix Has() functions (#1283)
82b25d0ec spinlock_bench.cpp: Fix wpiformat warning (#1282)
b44f27ddf SendableChooser: Rename addDefault and addObject for clarity (#1239)
fa78f30e3 ntcore: Make protocol revision more visible in docs (#1278)
46ae19d08 wpiutil: Add a mutex-compatible spinlock implementation (#1272)
77124a229 Twine: disable part of isValid() that causes spurious warnings (#1271)
1462a5bd4 Fix spacing and const correctness in sim (#1269)
44099d9a2 Update errorprone config for Java 10 (#1265)
c2ceebfb9 Add gyro and accelerometer simulator wrappers (#1183)
0a0d9245e cscore: Add connection strategy to sources (#1252)
7bd3f9f0b Change ButtonScheduler scope start method to public (#1254)
4801ae249 Replace ellipsis characters munged by wpiformat with three periods (#1256)
0e9172f9a Fix many errorprone warnings (#1247)
6db5f8043 docs: Set use_mathjax to true (#1251)
898076f69 docs/build.gradle: Use unix EOL (#1250)
195e10181 cscore: Use Twine instead of StringRef in API (#1244)
97a8f8f47 HAL: Add Doxygen module documentation (#1246)
9408fd517 Add Doxygen comments for namespaces (#1245)
00c2cd7da Improve JNI loading efficiency (#1224)
cbb62fb98 Fix errorprone so it is version locked and reports warnings (#1242)
a11fcb605 Add UseConcurrentHashMap exception to PMD and clean NOPMDs (#1243)
139b1720b Fix JavaDoc generation (#1241)
011f0ff53 UsbCamera: Make setConnectVerbose public and add test (#1240)
75a67202e Add documentation building to repo (#1238)
186e1dc54 ntcore: Documentation cleanups (#1237)
f2393feee cscore: Documentation cleanups (#1236)
66e35128c Use Pimpl idiom for CameraServer
6933fefe5 Use Pimpl idiom for Scheduler
fedf82812 Command: Use SmallPtrSet for requirements instead of std::set
eb64ea9fc MotorSafetyHelper: Use SmallPtrSet instead of std::set
826ed7fe3 SmallPtrSet: Fix makeIterator
a2d314b0d ConditionalCommand.cpp: Remove iostream include
6b37ca9f9 UsbCamera: Allow silencing of Connecting message (#1231)
0614913f1 Add a way to indicate a Sendable is an actuator (#1226)
5fafaf627 Add confirmation to SendableChooser (#1217)
397a296e2 Add a .controllable key as a standard part of Sendable (#1225)
1d9ed8f45 Fix SensorUtil issues (#1194)
bbb622aaa Revert "Use ASAN when on Clang (#1187)" (#1234)
8cbe7a625 cscore: Move CvSourceImpl placeholders into SourceImpl (#1230)
932308b49 MjpegServer: Add ability to set compression, etc in code (#1229)
939827825 cscore: Add properties support to VideoSink (#1228)
c9a75a119 cscore: Support multiple levels of JPEG compression (#1223)
04ee8dbe7 UsbCamera: In JPEG mode, get size from image instead of mode (#1222)
7fd7192b1 Disable UvGetAddrInfo Concurrent test on Windows (#1227)
63c1f80d6 halsim_ds_socket: Update tag parsing, and add rumble support (#1214)
d54c2665d Update UsageReporting enums and their uses for 2019 (#1218)
8aac46542 Add a SimpleBufferPool for uv::Buffer (#1215)
c78e1499d Remove appveyor artifacts, and only build 64 bit (#1212)
a34df5589 Switch DS Sim Sockets to use UV loops (#1211)
eb2c6e19f Add sim hooks to set match data (#1191)
c25d48fd0 HttpParser: Add Reset() function (#1210)
794403dce Add shim headers for headers moved to cameraserver and frc folders
d89b7dd41 Move CameraServer and WPILib headers into their own folder
31ced30c1 HttpParser: Change Execute() to return StringRef (#1209)
74a306d47 Add halsim_ds_socket to allow a simulated robot to talk to the real DS (#1180)
5bf582113 Cleanup docs in uv::Stream and uv::Tcp (#1207)
eed28a585 Add sockaddr_in overloads for uv::Tcp and uv::Udp (#1206)
435e026c0 uv::Loop: Add user-defined data (#1205)
739267d36 Add Reuse function to uv::Tcp (#1208)
85118a023 Minor fixes required to enable the simulated robots to run (#1181)
ae72c0b29 Clean up style issues in wpilibj_frcnetcomm.py (#1199)
b72885b4f Remove wildcard dep version from gazebo msg (#1203)
70b0d7cb0 UDPClient: Add receive functionality (#1204)
053ca47d4 Update gradle dependencies (#1202)
74efe5aaf Get halsim_gazebo building again (#1201)
fe5d7dd6b Move DS caching from user level to the HAL (#1143)
0b5df467e Update Native Utils to fix static allocations (#1196)
80134164a Move Java main function from library to user code (#1148)
76b26c2df HAL_InitializePWMPort: Check for MXP port (#1195)
f8635e8ab Update to gradle 4.9 (#1193)
4029b5d84 Update gazebo plugins to Gazebo 9 (#1189)
4c527b9b0 Add vscode intellisense settings (#1173)
caa03d23a Make JxArrayRef less error-prone for jxArray (#1190)
297863b17 Add HttpParser and UrlParser C++ wrappers for http_parser.
1992b67ee Import nodejs/http-parser.
e2314f352 wpiutil: Add C++ libuv wrappers (#1166)
340b26bad Use ASAN when on Clang (#1187)
7f000fecc Force a DS mutex release on shutdown on desktop (#1188)
76c901ce7 Add simple motor simulation classes (#1117)
57fc61407 Compile C headers in C mode, and fix cscore C (#1179)
89d15f061 Fix main function initialization (#1176)
f5b1028b5 Fix race conditions in command tests by increasing the delay time (#1178)
ad3e2d7d3 Make HAL headers C-compatible (#1177)
3818a8b3b Update to gradle 4.8.1 (#1174)
59e8b6026 Add HAL Documentation (#1132)
de5d7d3c1 Only use priority_mutex on RoboRIO by default. (#1172)
ebd41fe0b Bring back the gazebo plugins (#1063)
70960b025 Signal: rename Signal to Signal_mt and Signal_st to Signal.
c8afe9bc2 Signal: Optimize to use plain std::function.
1ecaaafa6 Discuss and include licenses for third party software. (#1101)
33a01b314 Add maven documentation (#1140)
1d8456e2b Move FRCNetComm.java into generated directory (#1168)
b5bacc09a libuv: Silence cast-function-type warning on GCC 8. (#1169)
876c65047 wpiutil: Add a signal-slot implementation. (#1163)
3eae079db Add PDP usage reporting (#1167)
122fdf48b libuv: Hook up to build.
d94f49b3b libuv: Silence clang compiler warning in uv-common.c
39670fc9c libuv: Add pragmas for win32 libraries.
6f0d50b9c libuv: Avoid conditional-true compiler warning.
873b2ed13 libuv: Add missing casts.
321c144d2 libuv: Remove extern "C" from uv.h.
13e1af259 libuv: Remove MSVC 2008 stdint from uv.h and uv-win.h.
9d7792ead libuv: Remove aix, os390, and sunos from uv-unix.h.
6d93d3c25 libuv: Rename source files from .c to .cpp.
156822dbc Import libuv 1.21.0.
208f82d6f Revert "Add libuv dependency (#1109)"
a818c7fd4 Add loop timing to IterativeRobot and TimedRobot (#781)
50b13d2f3 Convert UnitTestUtility to a JUnit 5 MockHardwareExtension (#1153)
b7807bf9d Clean up Command container iteration code (#73)
ea7d11b1d Twine: Make isNull() public. (#1162)
212f378d0 Replace globalError in ErrorBase with a global set of all errors (#615)
2faba39b5 Change wpilibc artifact name to match standard (#1158)
064989f2e Fix projects having different version numbers (#1113)
6b1b4796c Remove Link Script (#1159)
1ebb83e0f Remove explicit close() from Gyro interface (#1152)
9108a9359 Switch non-integration tests to JUnit 5 (#1120)
c7e97f45f Add RobotPeriodic functions to default templates. (#1149)
5af85dd1b Explicitly states the gyro direction contract for the Gyro interface (#1151)
b20158015 Update java robot class docs (#1150)
b1bb63f9a Add ADX Simulator tests (#1142)
056e68f2a Use new CAN API for PDP (#1081)
f6e4df6a1 Remove OS Serial Port (#1141)
0cde67143 Upgrade to gradle 4.8 (#1136)
1f9645afe Fix CAN API reads (#1139)
86285b427 Removes MSVC 2013 Shims (#1130)
e548a5f70 Update and enable PMD 6.3.0 (#1107)
8eafe7f32 Solve some safety issues with RPCs (#1127)
6aebba545 Import MapVector from LLVM. (#1128)
664a3c246 ntcore: Fix C API polling array returns. (#1126)
321dfaf0a Remove non-existent directory from .styleguide (#1124)
8373e0361 Made Controller interface public (#1123)
8c680a26f Moved C++ comments from source files to headers (#1111)
d9971a705 Throw UncleanStatusException rather then RuntimeException (#1114)
85fe722f4 Fixes JNI files not getting cleaned on rebuild (#1121)
c04f463b7 Makes FRCNetComm interfaces static final classes. (#1118)
307da3ad2 Simplify allocation of JNI global classes and exceptions (#1110)
39f80730d Disable broken ntcore tests using JUnit 5 (#1116)
35cfe0d92 Add comment to FRCNetComm.java noting that it is autogenerated (#1115)
8d218dbca Add support for unit testing from GradleRIO for C++ projects (#1094)
938f83514 Update things deprecated in gradle 4.8; remove unused function (#1093)
1dc55c03d Add Windows PDBs to release zips, and skip strip on mac (#1092)
859b457c3 Add libuv dependency (#1109)
8958c4eab Fixed wpilibj_frcnetcomm.py and added invocation to Travis (#1106)
7c9517ce5 Fix gain encapsulation in LinearDigitalFilter (#1105)
5bf9720cc Use externally built Google Test (#1108)
d1587ed2c LICENSE.txt: Update copyright date to 2018. (#1100)
5fcb67aaf Fix Checkstyle (#1095)
2e5fece59 Add utility class tests (#871)
863cfde39 Adds tests to ensure all examples have matching item in json file (#1079)
c4728d291 Makes CAN API initializer setup HAL (#1084)
fb45a5b31 Allows passing in the main robot class from the command line (#1091)
86c1f8ae5 Fixes initialization when not using the provided main (#1085)
381c25c57 Fixes interrupts not getting closed properly. (#1088)
62d5301b1 Changes notifier to be closable. (#1090)
40cc743cc Enable checkstyle on cscore, ntcore, wpiutil (#1032)
ecfe95383 Made SensorBase a utility class and renamed it to SensorUtil (#813)
ba93f79d8 Fix mac builds (#1087)
dcc276484 Default to requiring frc namespace for wpilibc. (#972)
cbaff5285 Implements AutoCloseable for types, replacing free() (#1048)
a2ecb1027 Update LLVM to latest upstream. (#1080)
680aabbe7 Add new CAN API (#1036)
55b0fe008 Fixes JNI symbol check on VS 2015 (#1078)
8b8c3d546 Updates NativeUtils dependency (#1077)
38a7786f2 Remove spotless (#1074)
df182f382 PIDController now supports composition (#976)
5cc757357 Updates JNI to support embedded jni libraries for easier setup (#1075)
17401e10f Add setting to invert the right side of the drive (#1045)
73439d821 DriverStation: Fix getStickButtonPressed/Released (#856)
72a79aac5 Fixed the names of the arguments to some C++ drive classes (#1070)
c89678971 Replaced the START_ROBOT_CLASS() macro with a template function (#1050)
64b03704f Rename Joystick default channel constants (#904)
630fc55bd Implemented synchronous PID controller (#993)
f90e429bf Add removeAll to preferences (#987)
2e0709f05 Add spotless to check line endings (#1055)
11d46713d Fixes pessimistic std::move (#1069)
ef442d775 Refactored DriverStation.java unplugged message handling to match C++ (#808)
3e6c3c3e9 Replaces Timer in PIDController.java with Notifier (#878)
8d57b73b4 Fixed naming convention of static variable in TimedRobot.java (#876)
d8c8643b5 Format HTML and XML files (#944)
adb609835 Removed extra newlines after open curly braces (#935)
938d5379e Adds command examples to built examples (#1062)
7cd15aa04 Re-enable JsonIteratorObjectTest.KeyValue.
f8ed48af9 Update json from upstream version 3.1.2.
c274d1790 StringRef: Add comparison operators against const char*.
6699f8636 Make most StringRef functions noexcept.
c2b1ed3ed ArrayRef: Add value_type typedef like std containers.
2c27ad073 raw_istream enhancements.
31bb55c31 Add std::vector and unsigned variants of raw_ostream.
dd4230d74 StringMap: Add decrement operations to iterator.
cff475c1f Moves C++ templates and examples to match gradle setup (#1065)
d564e19ef Only prints the debug binary message once per build (#1066)
1d6eb629a Adds Objective C++ Build capabilities to cscore mac (#1029)
406e18663 Switches NotifyListenerVector to SmallVector (#1004)
ab70220ec Makes SPI edge changes more obvious (#1056)
560123ab7 Fixes command folder name in templates.json (#1061)
4e1964156 Removes 32 bit configs for linux and mac. (#1060)
5ff3d837b Fix compilation with GCC 8 (#1051)
74d7107ac Add AppVeyor Build Support (#1013)
e21a246a4 Make the HAL self initialize when ever any initialization function is called (#1012)
59a8e9da5 Fixes gradle wpilibj classpath for editors (#1047)
795c60da0 Adds a testCpp task to all projects (#1014)
f3db32911 Enables MyRobot project (#1028)
f07799c67 Disables unstable and failing unit tests (#1057)
eec4f53a6 Forces CRLF files to LF (#1054)
01d8d0c79 Moved C++ header includes out of extern "C" and added missing C header includes (#1053)
6729a7d6b Run wpiformat on merged repo (#1021)
0babbf317 Adds CMake Readme (#1042)
337e89cf6 Adds JNI Simulator interface and updated Sim API (#1002)
104637134 Fixes some JNI issues with method calling and class storage (#1043)
665a6e356 Allow users to feed the watchdog contained in drive objects (#1044)
b7ea481bf Notifier: reset updatedAlarm before waiting (#946)
7a34f5d17 Check for nullptr return from malloc, calloc, and realloc. (#1023)
e8d5759d9 Actually have the cscore examples build, and makes them build only on linux (#1030)
eeae84c71 Adds cscore examples to builds (#1027)
dab6f40b4 Moves examples.xml to json, and adds template json (#1026)
5c2c5ccd0 Remove atomic static shim. (#1020)
8cbfe35bd ntcore: Remove MSVC 2013 shim. (#1018)
954f8c40f Adds CMake Builds (#1015)
6a49173ce Update cscore examples for llvm to wpi rename. (#1016)
1043aef7f ntcore: Make header actually a C header (#1007)
f7bcf5305 HAL: wait for all objects to release handle before freeing (#1011)
6a159c5bd Miscellaneous cleanups (#1008)
a098814ea Removes the make_unique shim for c++11 (#1010)
a28832e52 Add backwards compatibility shims for old wpiutil headers.
f84018af5 Move entirety of llvm namespace to wpi namespace.
93859eb84 TimedRobot now uses the Notifier HAL API (#942)
e7cf6bf7c Fixed wpilibj GenericHID.getType() (#969)
a8fd88840 Revert "Uses ComputeDigitalMask function across HAL DIO (#837)" (#1005)
c84bd744c Uses ComputeDigitalMask function across HAL DIO (#837)
11b99a016 Removes old version files from git ignore (#1003)
dfa46cbdd Fix typos, keep formatting consistent. (#974)
91151e33b Add out to gitignore (#988)
2ed9ae165 Removed unused ROBOT_TASK_PRIORITY constant from RobotBase.java (#991)
fdfea3516 Fix JavaDoc tag (#995)
47783842e Fix JavaDoc tag in Differential Drive (#996)
7f88cf768 New 2018 and later build setup (#1001)
cb2c9eb6d Remove gmock and builds from utility libraries (#999)
4a1e52075 Merge cscore into allwpilib.
b3aa659f9 Merge ntcore into allwpilib.
4870d83ad Merge wpiutil into allwpilib.
7210a8fd2 Prepare ntcore for merge into allwpilib.
0f947613a Prepare wpiutil for merge into allwpilib.
ea73c10cd Prepare cscore for merge into allwpilib.
6d3d52f92 Fix GetJpegSize() and JpegNeedsDHT() to handle SOF as first tag. (#127)
dbd1f1781 Ran wpiformat (#126)
96e9a6989 Improved Command-based examples (#906)
14228d82f Adds Direct port name Serial API (#956)
040731447 Add FPS and byte count telemetry measurement for sources. (#125)
5175829ba PWM: Use getRaw and setRaw for Sendable "Value" property. (#963)
9d7293734 SendableChooser: Do not automatically add to LiveWindow. (#964)
1e5ec362f CameraServer: catch VideoExceptions in video listener. (#949)
7bb3e4efc Made documentation for RobotDriveBase::SetDeadband() clearer (#953)
67de595c8 ADXRS450_Gyro: Add null check around reset (#948)
82152e90f Adds defaults to PWM config (#961)
1e7d43989 HAL Notifier: Don't disable the notifier manager when the last handle is cleaned up (#960)
71d06a1a2 HttpRequest: Don't reorder parameters. (#73)
698feff2f MjpegServer: Support limiting FPS. (#123)
3ef9ffaf3 HttpCamera: Force reconnect when SetUrls() is called. (#122)
3025a182c Fix two bugs in client synchronization. (#270)
febc41c85 Fix Travis CI wpiformat install (#72)
627ca6db7 Fix Travis CI wpiformat install (#121)
c80b0de2c Fix Travis CI wpiformat install (#269)
979984fa6 Fix Travis CI wpiformat install (#947)
57e9fb33d Fixes C++ SendableChooser using invalid temp variable (#945)
f5a292dad Adds TriState JNI entry point (#938)
4e9e7ec8f ParseHttpHeaders: Make case-insensitive per HTTP spec.
77d6c1174 Invert right side motors in MecanumDrive sendable (#933)
c69b8f00d ReadJpeg: Don't read past the end of the image. (#119)
0542b50f7 Work around fallthrough warnings on GCC 7+.
1077ef9fb Adds compile task (#118)
67f9c9a5b Fixed TimedRobot.java hanging if an exception was thrown (#926)
f720cbb12 Switches CtreCanNode to use locking and std::chrono for time (#909)
64a7e57fe Added output normalization to DifferentialDrive::CurvatureDrive() (#924)
5ca00dddb Added TimedRobot::GetPeriod() (#915)
120ceb342 Fix channel reassignments for C++ Joystick twist and throttle axes (#903)
5cbafc138 Updates to image 17 (#913)
39d1650d5 Fixes double to int to double cast in encoderJNI (#918)
02336fc47 Makes FMS data never be a null string. (#900)
c00848c06 Fixes indexed classed handle resource (#899)
738a1c015 PIDController: setContinuous should only check input range if continuous is true (#896)
48ae6c954 Publishes match specific data and other FMS info to NT (#874)
07f70cf78 Fixes control data packet delay (#875)
e4e1eab41 Fix cancel of inner commands in ConditionalCommands (#858)
0e8ff4663 SpeedControllerGroup: Call set() from pidWrite()
54a0a7654 Link to replacements for RobotDrive in JavaDocs (#879)
59f938b58 Invert when getting motor speed in SpeedControllerGroup (#886)
551388845 Fix PIDController with Continous and no Input Range set. (#883)
02b661504 RobotController: Make getBatteryVoltage() static. (#869)
1f4822f33 Replaced Talon motor controller in DifferentialDrive class docs with Spark (#868)
bb38ef564 DifferentialDrive: Invert right motor in LiveWindow. (#867)
ca36d1dce Adding callbacks for notifying when the distance per pulse changes (#861)
ee33296e1 SmartDashboard override .name entry in putData(String, Sendable) (#866)
4376c94dc Updated copyright year
19f7a5f10 Set up wpiformat
4514ff807 Removed extra newlines from beginning of Java classes (#264)
b66d72f5c Updated copyright year
c6f6b352f Set up wpiformat
0ef980336 Update copyright year to 2018 (#864)
eedb8910c Removed extra newlines from beginning of Java classes (#859)
882399c65 Update copyright year to 2018 (#116)
8346caed9 Move subsystem command metadata to metadata key format (#863)
55b6764d5 Fix bugs in simulation libraries (#853)
2c4faee66 Fixes -Og compile and strip binaries (#838)
228728106 Fixes linux and mac builds to -Og, and strips binaries (#261)
cd4b7b6cc Switches to -Og instead of -O2, and strip binaries on linux and mac (#63)
da5458a2d Updates to newest build setup, fixing -Og and strip binaries (#115)
8d1dee16b Add DriverStation NetworkTables HAL Extension (#829)
76b182600 Add Low Fidelity NetworkTables simulation extension (#823)
c647a801a Add Encoder Index as a child (#857)
02131639b Add Digilent DMC 60 Speed Controller (#855)
566e28369 Make setDefaultCommand public (#854)
40eb6dfc9 Fix SmartDashboard PutData to hook setters. (#851)
a3e5378d1 SPI: Check for null (#850)
691741cfc Fixes non public static methods in RobotController. (#852)
166d9e01b Add PWMVictorSPX (#842)
7eab4371f Adds TriState DIO functionality to the HAL (#835)
de134a5c6 Add deprecated shims for LiveWindowSendable and NamedSendable. (#834)
7f074563d Add support for automatic SPI transfer engine. (#836)
d3dd58636 Revert "Fixes SPI bad chip select (#818)" (#822)
9c8510559 Update to image 2018v16 (#833)
86ac70a12 Fix wpilibcIntegrationTests RobotController warnings. (#832)
3c3a448d4 Deprecate SampleRobot (#472)
8744511f1 Fixes some methods in RobotController not being static. (#831)
7729dd972 Fixes JNI symbol check (#830)
8b7aa6109 Adds RobotController class (#828)
88a6b4ac3 PIDController::InitSendable(): Use double, not bool for double values. (#827)
217b1a225 VisionRunner: Add stop() function to stop a runForever() loop. (#826)
d2e7a90f4 Removes statics from hal sim (#825)
8bd48d6c3 Switches HAL to manual instead of static initialization (#824)
1fa0adb09 Removes MSVC and GCC old version workarounds (#821)
f615e68a4 Require GCC 5 for wpilib build (#820)
aa4f0ef4f Start using the new FRC compiler define (#797)
b42285fdd Fixes SPI bad chip select (#818)
8106fbdbe Removes custom CONFIG_ATHENA compiler directive (#796)
942ba5176 Reclassified NetworkTables headers as "other library" (#775)
33a08d5b3 Add halsim unit tests (#783)
4e3af0756 Removes workarounds for issues in image 10 (#816)
5078f6c92 Update to image 2018v13 (#815)
899892c11 Change Utility to use Twine.
54326311a Use Twine in error checks.
fe53dd2f2 Use Twine for error message inputs.
ab137abab Use llvm::Twine across C++ Command structure.
001dedf3b SmartDashboard: Use magic static for static initialization. (#812)
f9bece2ff Update LiveWindow to provide continuous telemetry. (#771)
3befc7015 Make MotorEncoderTest use LinearDigitalFilter's reference overload (#811)
de63e1c8a Fixed race condition between PIDController enable/disable and PIDWrite() call
a76b1aa80 Reduced scope of PIDController's critical sections
350b741ad Cleaned up SampleRobot template and added warning about disabling motor safety (#766)
64bfdc1a6 Bail out of the integration tests if enable fails (#792)
65cc85f68 Add reference constructors/factory methods to LinearDigitalFilter (#810)
6a00dc797 Re-Add PacGoat java example (#802)
e9e407a87 Replace C identifier lists with (void) (#809)
59c4984ed Deprecated internal filter of PID controller (#746)
b428d1e4b Remove CANSpeedController interface. (#806)
099436459 Fixes cross module base class statics (#779)
0b8d3f026 Update SampleRobot template to match comments (#768)
52eba45c5 Add missing documentation for squaredInputs for RobotDrive (#805)
9090a82ef Fixes halsim sources and headers zips (#804)
4e0ed7986 Fix no return warning in jni_util.h (#62)
2a69a4c7d Revert #780 (don't kill FRC_NetCommDaemon). (#795)
9d2393f97 Waits 10 seconds between running the C++ and Java test (#798)
dae619b00 Add error reporting to AbstractComsSetup test initialization. (#800)
9c06d2878 Update to latest native plugin (#799)
e3a2abdf9 HAL_SetDigitalPWMRate(): Use same logic as LabView. (#794)
7867e906e NidecBrushless: Have disable() call PWM.setDisabled(). (#763)
65a044f63 Fix HAL_CleanNotifier race. (#793)
cbd08a1e1 Add tests for equivilance of RobotDrive and DifferentialDrive/MecanumDrive (#732)
e308dd28f Fix javadoc link in Solenoid.java (#789)
fa0b4428e Runs DS enabled loop in process (#785)
6767afd40 Adds a UDPClient (#60)
26a36779a Performed cleanup of Timer's free functions (#776)
dfc0656e5 Fix wpilibj FilterOutputTest null pointer exception (#778)
0f3f5218a Kill FRC_NetCommDaemon as well as robot programs. (#780)
34c44b7ae Improved Drive docs and fix implementation bugs (#774)
7a250a1b9 Implement PCM One Shot feature. Fixes artf4731 (#539)
a338ee8be Add PIDController Wrapping (#601)
c9d440f33 Fix Java Compressor test. Make limits same as C++. (#772)
aa2de65ba Use Twine instead of StringRef where appropriate. (#259)
a00b2449d Removed unused includes and replaced Ultrasonic's std::set with std::vector (#767)
5c659fdcd Deprecated PIDController::SetTolerance() (#764)
d36d72bd4 Fixes MotorSafetyHelper locking and race conditions (#762)
614093c0c Fix documentation for getMatchTime in Timer class to match DriverStation (#761)
877c7f51c raw_istream: Don't forward declare Twine et al. (#59)
912b74151 Use llvm::Twine instead of llvm::StringRef in several places. (#58)
f73db4a49 Twine::isSingleStringRef(): Support CharKind. (#57)
12c4418bd Added callbacks for CAN (#757)
0431cf97f Cleanup PIDController (#597)
ba879f466 Cleaned up variable names for std::lock_guard and their associated mutexes (#759)
96de0b5b1 Variable name fixes (#758)
85157a56c CircularBuffer now uses an idiomatic interface in C++ and Java (#421)
029246ed2 Replaced extra constructors in LinearDigitalFilter with llvm::ArrayRef<> (#755)
6377ab774 Allow deprecated warnings for tests (#756)
05e581f40 Fixed crash in wpilibj SampleRobot (#753)
c73dd807e ErrorBase: Remove last use of sstream and iostream. (#750)
7a0dd9baa Add return-to-zero test for LinearDigitalFilter moving average (#751)
11f37683c Added constructor to PIDController that takes references instead of pointers (#745)
5af0c9c10 Replaced const variables with constexpr (#731)
259461aee Added PIDController::GetAvgError() back in (#749)
d214b3678 Change HAL notifier to polling. (#627)
4a07f0380 PIDController class now uses LinearDigitalFilter for filtering velocity instead of internal queue (#38)
cf828ca85 Upgraded clang-format to 5.0 (#103)
7847c6923 Update for wpi::Now() change to microseconds. (#113)
7f46b50b2 Unify WPI_Now and HAL_GetFPGATime. (#743)
0e4a1c5da NetworkTable: Add key utility functions. (#256)
551504e77 Update documentation for Now() for 1 us steps. (#258)
85e83f1bb WPI_Now(): return microseconds, and make backend replaceable. (#56)
7eac3fcbd Java NetworkTableEntry.setValue() and kin: Handle common Java types. (#257)
303c259b8 Simulate ADX* family of accelerometers and gyros (#688)
cd1dbb1e3 Adds a const buffer listener (#742)
a20474bfc Update sensors to not use direct byte buffers.
479d0beb5 SerialPort: Use byte[] instead of ByteBuffer in JNI.
b93aa176d AnalogInput: Remove byte buffer usage.
9021b37fd I2C: Provide byte[] JNI interfaces.
6307d4100 SPI: Provide byte[] JNI interfaces.
e9b0b9d8f MjpegServer: Use sink name in title. (#112)
cad1b9413 Add exposure quirk for LifeCam Cinema. (#111)
3324bcc5c Use magic statics instead of ATOMIC_STATIC. (#109)
20c8d29ae Fix wpi::mutex usage in SetVideoMode().
110726c5b Adds support to test library for JNI testing (#54)
df7c1389d Remove ni-libraries libi2c and libspi.
6accc31ee HAL: implement I2C and SPI directly instead of using i2clib and spilib.
f56ec10bc Only return lower 32 bits of FPGA time (#741)
e5e6d6a19 Ran formatter based on styleguide#95 (#737)
c663d7cd1 Reflowed comments and removed commented out code (#735)
1e8d18b32 Upgrade Travis CI Python version to 3.5 (#740)
c2d95db3a Fix PreferencesTest. (#739)
7db60f8e7 CvSource: Implement SetVideoMode(). (#104)
b3f1e7431 Ran formatter based on styleguide#95 (#108)
dd7563376 Force load OpenCV and cscore libraries on program initialization (#716)
14fcf3f2f Simplified PIDController integration logic (#645)
020ee227d Move ctre headers to hal/src so they aren't user-visible. (#728)
4d559f385 Use wpi::mutex instead of std::mutex. (#730)
e301adb22 Use wpi::mutex instead of std::mutex. (#105)
3438a1734 Use wpi::mutex instead of std::mutex. (#254)
e4deda5cc Fix SafeThread bad merge. (#53)
80618a2e6 Use wpi mutex and condition_variable. (#52)
e45b6e0f6 Fix typo in priority_condition_variable. (#51)
86d4899a5 Make NetworkTable constructor private/package-private. (#253)
9d8a508cd Add priority_mutex and priority_condition_variable. (#50)
c9ead29f4 SafeThread: Simplify m_lock assignment. (#49)
35d68d2a3 Adds ability to simulate joysticks and event info (#727)
7007725d9 SerialHelper: Check error_code to prevent infinite loop. (#725)
0c83cad70 Upgraded clang-format to 5.0 (#431)
0001047b8 Add NidecBrushless to WPILib.h (#724)
f77fd1eca Set SO_EXCLUSIVEADDRUSE for server sockets on Windows. (#44)
5a5e75392 LLVM path: Don't follow symlinks. (#48)
c88525136 Fixes HALSim_Print build to be the standardized pattern (#721)
42096fac3 Ran formatter (#722)
6be9e69d1 Switch Command Templates to TimedRobot (#719)
7bbd13d91 Adds match specific calls to Java and C++ (#720)
3e4e5261f Remove no warnings on winsock and scl issues (#47)
cf4afb6fe Adds get header task (#46)
55fa1e5e7 Adds header task (#101)
2225c4fee Fixes warnings for casts in sim interrupt functions (#718)
c10165541 Adds warning prints by updating native plugin (#100)
51165ba0a Updates native plugin to add warning prints (#251)
f03b31f43 Adds warning printouts to gradle by updating native plugin (#45)
7efab4c43 Replaced ternary operators with if statements (#346)
c8e44256e Uses NI provided function for SetOccurDataRef rather then importing the symbol ourselves. (#714)
f3cd883c5 Add Nidec Brushless motor. (#705)
f34c736fb Adds warning prints to native library builds (#710)
127648996 Removed uses of deprecated functions (#709)
8b2e656bd Fixed Java velocity PID not calculating result when P = 0 (#717)
6e9d1f55c Updates image to 2018 v10 image (#713)
f4dce9e60 Fixes receive size in Java I2C (#715)
b9aabc71b Fixes publishing basenames for examples to make combiner script work. (#712)
0d5477236 Add IDEA plugin to root project (#707)
faf134a67 C++ examples no longer use deprecated APIs (#703)
c24e75540 Fixes java example publishing and xml file (#706)
bee9f1cb1 Adds header task to print out all headers (#704)
45d48d6b5 Cleaned up C++ examples (#672)
6401aa1fd SerialHelper: Use llvm path functions instead of popen. (#702)
8e797a1a1 Import filesystem directory iterators from llvm. (#43)
6af4940c2 Adds HAL calls for match data from DS (#691)
237b2df82 Add .type metadata to preferences table (#701)
a70687aae Improve error reporting for the new TCP netconsole. (#700)
fbfe85568 Fix preferences test (#699)
595b1df38 Fixed minimum number of joystick axes (#696)
fd32350dc Assign received sequence number on receiving an unknown entry (#250)
b9c8ebeff Ran formatter (#99)
efc7770e9 Fixes NPE in DriverStation initialization (#694)
21585f70a Added functions for detecting button press and release events (#626)
c33fca34e Added TimedRobot template (#673)
de95f08a1 Adding call to notify program started (#692)
90f99dc57 Adds PWM to LoopTiming and CycleStart HAL calls (#693)
f34332643 Uses new FPGA calls to get 64 bit FPGA time and 64 bit PWM cycle start time. (#687)
4ab095e9c Fix formatting in CameraServer.cpp (#689)
541753c81 Updates to 2018 v9 image (#686)
f02bb058b Set the llvm standard output stream to be unbuffered. (#678)
12e96c6f1 Add usage reporting to CameraServer (#682)
b65447b6f Fix spelling typos (#595)
9945a5b3c Ran formatter (#681)
ce4c9edd1 Fixes halsim_print to not try and build when the 'onlyAthena' flag has been set (#685)
a6e6ae41b Add function that can register all the HAL callbacks at once (#646)
f4e2e41aa Remove leading '*' from license and rename to LICENSE.txt (#596)
1e528669f Adds publishing for examples and templates so they can be grabbed from eclipse (#674)
a1ea44840 Adds JNI call to get CANStatus (#677)
9dc1de1d0 Specified angle units for Vector2d rotate() function. (#679)
0521d8504 Moved comment after include line so include order is determined properly (#680)
3c842d823 SPI and I2C simulator (#662)
ec12b0ffe Add wpiformat to Travis CI (#98)
7fd594748 Fix include guards (#97)
be77f9cb2 Pull request for the Extensions interface only (#655)
2fc60680f Remove RedundantModifiers (#578)
0291a95f6 Add cpp examples (#659)
66002d6ca [WIP] Move examples to allwpilib (Java) (#569)
a7e9ac106 Fix capitalization of class names in PIDToleranceTest.cpp (#588)
434d60592 Adds HAL_Initialize to ErrorBase constructor, and makes HAL_Initialize properly reentrant (#668)
ee2074725 Adds WPILib JNI shared debug file publish. (#671)
f4779379c Added brace comments (#670)
877a9eae1 Add SpeedControllerGroup (#362)
b68e1c557 Added brace comments (#96)
ded1beb94 Removes specific raspbian, armv7 and armv8 cross builds (#249)
a6c7789b5 Removes specific raspbian, armv7 and armv8 cross builds (#41)
24752a975 Removes OpenCV dependency from wpilibJNI (#667)
e1dc09951 Updates to the 2018v5 image (#665)
f0cc62324 Change metadata format to dotfile, make certain entries metadata (#666)
ba3a85d0c Changes TalonSRX to PWMTalonSRX in wpilibj and wpilibc. (#656)
79919a5f1 Moves wpilibc link script into shared folder (#660)
94ea5bfb0 Move published wpilibj JNI shared artifact into shared/ subfolder (#658)
24680bbd5 Change link script artifact id (#657)
73f8412b4 Correctly handle negative waitForXQueue timeouts in Java.
529d7f5fe Initialize logger min level from logger_impl.
a6c1e18ae NetworkTable.containsKey(): Always return false on empty key.
8a37b81f4 LoadEntries: Don't emit NOTIFY_FLAGS. (#247)
f81b6fbcd Fix handling of deleted values in several places. (#241)
7fab0e0ef Depend on wpiutil 3.+, ntcore 4.+, cscore 1.+ rather than just +. (#651)
223e61df2 Depend on wpiutil 3.+ rather than just +. (#95)
303df626a Depend on wpiutil 3.+ rather than just +. (#235)
9e8ad778d Update wpilib-version-plugin to 2.0. (#39)
1f18cc541 Add SaveEntries() and LoadEntries(). (#233)
e68a71022 Move immediate connection notification logic into Dispatcher.
e4a8bff70 Move immediate entry notification logic into Storage.
10982e027 Don't actually remove deleted entries from m_entries. (#239)
dd66b2384 Remove priority mutex (#644)
19addb04c Split RobotDrive class into a class for each drive type (#552)
abb66d3e4 Replace FRC_NetworkCommunication CAN in wpilibj with HAL CAN (#650)
db4981f16 Java now asks the HAL if it is a simulation or not (#647)
8edc02b06 Update README for new build system. (#232)
4b2aaee9e Made package local methods in NetworkTableInstance public (#234)
9fdb33b6a Switches native linux arm build to be `nativearm` arch (#38)
6e1be897d Ran formatter (#643)
423d8f686 Add missing usage reporting. (#639)
eb38204d1 Update the README to note the need for gcc 5+ and Visual Studio (#640)
1711291cd Fix Travis build (#641)
3faecdb35 Add SHA1 algorithm implementation. (#32)
57ba58917 PreferencesTest: Use new NetworkTables API. (#636)
2249a8bac Adds a HAL wrapper around the CAN API's (#623)
c572e6a30 Adds the rest of the data needed for a publish on jenkins (#628)
e444b6015 Updated remaining .styleguide files to new config file format (#635)
db96f41ad Log.cpp: Use raw_ostream and llvm path functions. (#93)
38dbed8e0 .styleguide now uses generalized config file format (#621)
10fbf17d4 .styleguide now uses generalized config file format (#94)
34c18ef00 Remove getTable from wpilibj Sendable interface.
0d4fde17e Remove GetTable from wpilibc Sendable interface.
040a8c6bc Update wpilibc to use new NetworkTables package and interfaces.
4e80570c4 Update wpilibj to use new NetworkTables package and interfaces.
91529cc43 Update NetworkTables for new API.
ef8580969 Fix bug in raw_fd_istream::read_impl(). (#30)
95bce5d65 Add more storage incoming unit tests.
cedbafeb2 Add INetworkConnection interface for unit testing.
7c1d2f4bc Improve client connection synchronization behavior.
8099d6dbd Refactor Storage ProcessIncoming().
8e01b68cf Switch Storage save/load to use raw_ostream/raw_istream.
d707a07f8 Refactor Storage load and save functionality.
5ab20bb27 Implement independent instances.
8125a179f Only include .cpp files in testsuite build.
041563f8e Change UidVector to be LRU with a reuse threshold. (#29)
ef3267833 Fix IterativeRobot/TimedRobot RobotInit(). (#633)
a5ef50c9e Add documentation for PeriodMultiplier (#632)
6eee45789 Renable javadoc fail on error (#631)
fb6d7b347 Fixes javadocs (#630)
2d78fdabb Fix recieve typo in JNI names. (#629)
94c31ceeb Gets wpilib compiling with skipAthena and onlyAthena flags (#625)
c3f7c85f8 Adds wpi GetHostname function (#25)
12b2efa48 Log.cpp: Use raw_ostream and llvm path functions. (#223)
67d4da51e Restore travis support. (#608)
6e4f66cc8 Classify other libraries' headers properly and fix committed formatting issues (#620)
5d403a7b4 Makes an empty stringref have a valid Data pointer (#28)
3c88f94b4 HttpUtil has moved to wpiutil; use it. (#92)
06636a0e1 Set up wpiformat (#86)
a4e781a23 Fixed spacing and comment annotations around namespace and extern declarations (#587)
f151892db Contents of copyright line now has more standard ordering (#585)
c45fb73f3 Convert for loops to foreach loops (#592)
848280d1f Improve C++ Compressor documentation based on Java's docs. (#607)
8416b4e42 Add default parameter to StringRef to allow null termination on lengthed strings. (#27)
1a0ed61f7 Force the java dev library to be built during `build` (#24)
68501759f tcpsockets: Don't pull in platform-specific headers in headers. (#26)
f7016b359 HALUtil: Update to new GetJavaStackTrace function.
be58a279a Cross-platform JNI sometimes has jlong != int64.
909e6c485 Error.cpp: Use llvm path function instead of OS basename.
5c0b08f4f Turn off -Wunused-const-variable on Mac.
05d1cfa27 Add "override" qualifier to several headers.
ae675ae4e DriverStation: Work around missing pthread_local on Mac.
d3b636d07 fpga_clock: Don't use constexpr for time_points.
8b460f594 sim DriverStation: Fix missing includes.
bdfa32876 Fixed Gradle test task name (#617)
4a3472ebb Removed unused include in PIDCommand.cpp (#616)
9c804c135 Fixes builds with skipAthena and onlyAthena, and mac builds (#613)
2a9c454ba Cleaned up and updated .styleguide for new build system file locations and ran wpiformat (#612)
1a9a6c367 Fixes HalSimStaticDeps config and publishing (#610)
e1195e8b9 Update to 2018_v4 image and new build system. (#598)
dd85b1e51 Update googletest and googlemock to 1.8.0. (#90)
2d3cf1bdb Updates plugin to 1.2.12 (#91)
baa8021c7 Force dev java to be built during `build` task (#227)
133540f57 Switches to the new build system (#87)
43c103c0a Update googletest and googlemock to 1.8.0. (#23)
162ac787b Update googletest and googlemock to 1.8.0. (#226)
fa7d5bc02 Add UidVector (used in both ntcore and cscore Notifier). (#22)
9d4508812 Fix destruction order issue in SourceImpl. (#89)
7ef56de3f Fix mac builds. (#88)
0d76b3f30 Added gradlew.bat to .gitattributes (#225)
50ed55e8e Force OpenCV to 3.1.0 (#602)
92c4c49b0 Removes the custom platform include flag (#224)
855df5d67 raw_mem_istream: Add StringRef constructor.
c8d9cc7e5 Add filename constructor to raw_fd_istream.
1c1fbf14c Import LLVM openFileForRead and openFileForWrite.
9e4dc235d Connect to server in parallel rather than doing round-robin. (#205)
4bd8cf6f5 Native tests depend on native sources, so include in exportedHeaders. (#222)
d9c754c30 Add a java version of the dev app. (#218)
ea028a382 Add a cpp dev run task. (#219)
7d9e6b7e2 Move ReadLine into raw_istream class as getline. (#20)
17b5cace5 Base64: Add raw_ostream and SmallString interfaces. (#19)
2fa41b23b Add a java version of the dev app. (#16)
9f5f6111d Add a cpp dev run task. (#17)
078216412 Gradle 4.1 (#220)
5439fe7b1 Gradle 4.1 (#18)
55111ac35 Fix CORS issue with all requests (#85)
f0cc5d9ca Adds an all artifact to the published libraries (#15)
ccfeab5ac Adds an all artifact to the published libraries (#217)
d682295cc Miscellaneous cleanups for HAL, wpilibc, and wpilibj JNI (#589)
5e19c1881 Use diamond operator
617ff52f1 Use generics
7e011bda6 Add HTTP utilities. (#7)
8418c3912 Add Path and Twine components from LLVM. (#10)
5d3af62c0 Remove use of std::chrono_literals (C++14 feature). (#574)
4b8ef57a9 Remove networktables2.type Java custom container types. (#214)
d910b0b2a Remove deprecated throwing get functions. (#213)
80c8de7d6 Add dependency to jni task to fix 32 bit builds (#216)
7f776deae Fix ntcoreExe build model. (#215)
8209ba8a0 Move NetworkTable into nt namespace, with a shim. (#211)
25c8e873d TCPConnector: Add method to connect to server in parallel. (#6)
e24db75f0 Build updates to newest version of plugins and gradle, along with config file (#13)
5df746366 Remove wpiutil and update to the new build system (#210)
eb7331f2a Actually makes the right classifier
03bd0820c Adds a java classifier whenever jenkins is building (#12)
63768166e Updates to plugin 1.2 (#9)
bd899a7a7 Fixes mac RuntimeDetector (#11)
ddd5aeba1 Checkstyle 8.1 (#584)
301442ee4 Add json support to wpiutil.
de9dd1180 Include src/main/native/cpp when building test.
436ed4d1e Implement comparison operators for llvm::StringMap.
88aa273e5 license.txt: Add LLVM license.
d11d8409a license.txt: Remove leading asterisks
9385d1b6d GetJavaStackTrace: Provide llvm::StringRef excludeFuncPrefix. (#3)
b90653f3e Logger: Add def_func and min_level constructors. (#2)
1243cf04e Adds new build system to repo (#1)
3cfcbe9a9 Remove Scheduler.java from the sim path. (#564)
4f5b5b137 Adds gmock files
6bc793505 Suppress MultipleTopLevelClasses warning (#581)
3b44160cf Rearrange some method calls so methods fail faster (#583)
d9586c8d3 Fixes JNI aliasing issues involving ControlWord and AllianceStation (#575)
74df3fac4 Require non null (#580)
06321b8e8 Fix encoder sourceA null check (#579)
f3efb948f Switches CANJNI to use byte[] rather then ByteBuffer, and throws exceptions for invalid platforms (#571)
432c03bf6 Updated Gradle to 4.0.1 (#573)
bfd224278 Fix Java MyRobot linting (#572)
822ea6abc Made SpeedController class organization consistent between languages (#568)
97437ee58 Miscellaneous formatting and style fixes for integration tests (#570)
aa7fe0db2 Fixes unused buffer warning on platforms other then athena (#566)
237685d4a Fixes static initialization of HAL handles (#565)
abbe63027 Gets WPILib building on windows and C++11 vs C++1y (#561)
89d3b08e7 Added TimedRobot (#520)
f826216a2 Gets JNI compiling on all platforms (#563)
aa0a874ad Add periodic method to subsystems (#528)
2da26c057 Make HAL_Initialize timeout configurable, makes result a bool, and makes Java an exception rather than assert. (#557)
d34c84490 Fixed function ordering in robot base classes (#553)
68b63632c Removed functions that have been deprecated for at least one year (#551)
d2de94778 Remove memory leak in ConditionalCommand (#537)
4fd4a50d4 Update Preferences to not use deprecated APIs (#555)
9d9382071 Replaced STL streams with LLVM's raw_ostream (#344)
c57a7f0a4 Switches all notifiers created with the external API to be threaded (#546)
1d15fcd07 Prints error when HAL_GetFPGATime call fails in fpga_clock. (#558)
e824b1129 Adds way to reset and version all HAL handles (#545)
0cd03c66e Remove deprecated Task class (#550)
462b231da Adds function to HAL to free malloc'd joystick name (#543)
e4a918850 Fixed return value of SendableChooser<T>::GetSelected() for non-pointer T's (#512)
d472af351 Removes CAN.h from the HAL (#556)
98a587a34 Removed unused includes and added missing stdint.h includes (#549)
5fd996876 Fixed a typo (#547)
f43675e2b Add functionality for getting the current network mode (#202)
417cf33f9 Expose ITable paths (#200)
b433d98c0 Replace std::stringstream with llvm::raw_svector_ostream (#345)
7006672b0 Fixes hal shim from alias to using item (#538)
efec0c5cc Moves the HAL priority_ custom types to the hal namespace (#532)
16e71eac4 Fixes assertions to not crash StringRef (#534)
067b1f3ee Adds a way to get the native handle from SafeThread (#198)
3d2f41d08 Adds a way to get the native handle from SafeThread (#198)
68b62abb5 Adds chrono wrapper around fpga time (#527)
b2f347969 Switches SPI and I2C to use enums in the HAL for ports (#531)
67d62ba16 Travis now passes the current year to wpiformat (#438)
e1fc60b8d SolenoidBase functions are now static (#530)
f32e696fe Fix exception when getting a relay in kReverse Direction. Fixes #458 (#525)
fc81298fa Switches DS to use Occur callback rather than internal netcomm semaphore (#510)
5987cfeaa Fix SPIs to not set CS DIO pins into DIO mode (#504)
cd0ece451 Removes the semaphore class (#529)
dc9f85ebb Fix comment refering to old _val enum values (#524)
35a2055cd Improve documentation for WaitCommand and TimedCommand. (#494)
a1066776a Make SmartDashboard methods static (#523)
4dae74734 Refactor RobotDrive squaring (#390)
d348a5b94 Fixes WaitForInterrupt to return values matching enum (#503)
f0c413f40 Relay stores nt value in enum (#460)
846eee2f6 HashMap -> LinkedHashMap (#492)
0b9ff68da StartLiveWindowMode ITables use lambdas (#461)
7187e005d Add Checkstyle WhitespaceAfter check (#466)
a5cd24e7e Reduce scope of for loop variables to for loop initializer (#517)
dbe821bae Change the wrapper to use the all distribution with the sources (#498)
04486c647 CameraServer: Always update addresses when updating stream values. (#496)
2ee8ba936 SafePWM: Rename Feed to feed (#463)
b07782cd8 Removes StatusIsFatal checks from PDP (#493)
305450b6e Remove 'value' from PIDSourceType (#464)
fdd0b853e Fixes Base.h to work properly with MSVC 2015 and above (#513)
49de28d3d Add overloads for property types except enum (#73)
c34cf1176 MjpegServer: style fixes for root page (#72)
42facbb07 Make Java "struct" constructors public for testing/simulating (#195)
9f97cd61b Correct platform path for 32-bit Linux (#179)
1d025204e Fixed Spelling Mistakes in SmartDashboard.java (#506)
59133a7d9 Use Javadoc formatting for Java comments. (#66)
b484cbba7 Adds SinkFrameTimeoutCpp to def file (#61)
61e34621c Add GetNextFrame timeout to CvSink and MjpegServer.
5e9575de6 Add missing CS_SetDefaultLogger in cscore-jni.def.
12f759860 Corrects assumptions about return values from i2c-lib (#484)
c02d34dbf Allow resetting the logger back to default (#55)
8f97637b7 Fix handling of HTTP cameras that do not provide a Content-Length. (#59)
878d3a6f4 Publish video modes to webserver. (#58)
ef25bbde7 Fix SetExposureAuto. (#57)
1bdbb5ddc Remove usages of isEnable() (#483)
b573fb655 Fix param order in RobotDrive docs (#481)
b50a7bdbe Fixes memory leak for SPI reads (#474)
5a5f10dfc Fixes I2C read change size of pointer (#479)
8c3efa592 Increment reference count when creating VideoSource object (#53)
e6656326a Adds field IP to round robin list (#187)
e375b4a9f CameraServer: auto-increment startAutomaticCapture(). (#468)
ff141ab1f Fixes exception on camera creation without plugged in camera (#470)
cf8cab850 Fixes missing VideoProperty constructor. (#49)
b8537be21 CameraServer: Remove NT-driven settings. (#467)
db5dfa174 Adds sliders for all settable properties to the default webpage (#48)
13457d1bf Update setUpdateRate() documentation to match implementation.
8f1b034b2 Fix typo in JavaDoc (#462)
71d0a07e0 Fixes solenoid allocation error message (#455)
d32234249 Remove comments about Blue DS (#450)
8f8c4d3d9 HttpUtil: Allow "http" scheme to be mixed case.
d47bd1ecb Fixes embedded library name used for extraction in Java (#45)
e9fcb5381 Fixes Java artifact clasifiers when using custom suffix (#44)
3e2631f49 Fixes Java artifact clasifiers when using custom suffix (#183)
bc7ab1769 CameraServer updates (#446)
3c3236c5d Fix video mode notifications.
7a049c29b Added a conditional command that chooses 1 of 2 commands (#435)
23462ec7d Adds way to force publishing version from command line (#43)
ac56b0a33 Adds way to force publishing version from command line (#182)
1fc375b31 Updates the gradle wrapper to version 3.3 (#445)
eee7f3991 Allow VisionRunner to stop (#442)
b55c604c0 Updates Gradle to 3.3, and removes the Visual Studio 2015 workaround (#41)
a72f8f3bc Add new ARM Toolchain, add toolchainpath to README (#42)
2e3503517 Add OS level serial port (#426)
27c0405fc Reruns gradle wrapper to fully generate 3.3 files (#181)
b9e80ecfd Updates to gradle 3.3, and removes Visual Studio 2015 workaround (#180)
a3adb38be Adds support for specifying output suffix of arm packages. (#34)
593ba37c4 Java VideoEvent: Make propertyHandle public.
4ed78a84e Java VideoMode: Provide PixelFormat-taking constructor.
e893662c0 Adds new functions to def file (#40)
15e58acc7 Java SendableChooser no longer sets a default on addObject (#441)
5df78c520 Adds support for building separate armhf artifacts (#177)
f13f88688 Fixes arm example builds on windows (#39)
5dd8e4dc7 Adds defs for the C windows functions, and shims the 5 missing ones (#38)
5aa5e3e09 SendableChooser generic value (#433)
883fd5b06 For property events, provide property name rather than source name.
7ddbf2010 Create VideoCamera base class and move camera settings functions to it.
23135d7a5 Allow Sink.SetSource() to be given an empty source.
b91ab0b44 Gets builds working on Windows (VS 2015) (#37)
9a2ec13ba Fixes SetIntArrayRegion template for windows builds (#178)
78995f5cc Fixes SetIntArrayRegion template for windows builds (#178)
f225c4773 Fixes gradle publish with no flags. (#35)
25ae7b2c2 Upgraded Doxygen gradle plugin to 0.3 (#440)
4b6dc9583 Corrected order of access specifiers in MockCommand class (#436)
64eab1f7b Fixes missing Extern "C" from HAL Threads (#437)
bac4b3d5c Fixed gradle publish with no flags. (#176)
182f57216 Adds project and classpath ignores (#425)
65b091a39 Call SmartDashboard::init() in RobotBase constructor. (#428)
ed1a94531 Update license headers to 2017 (#434)
e3f99a4a2 JStringRef: ensure string is null terminated so c_str() works correctly.
df7d3261c JStringRef: ensure string is null terminated so c_str() works correctly.
95ad4783f Revert "Fixes TCPAcceptor able to use an empty string (#172)"
b5b089922 Revert "Fixes TCPAcceptor able to use an empty string (#172)"
9a0a1baa6 Fixes TCPAcceptor able to use an empty string (#172)
6ad9f45d9 Fixes TCPAcceptor able to use an empty string (#172)
4e12ffb0a Update the wpilib version plugin (#423)
7ec223d44 Update WPILib Version Plugin (#33)
ed9e83722 Updates the wpilib version plugin (#171)
b9a08e826 Fixed entry comment (#169)
58931e1d3 Be even more permissive on valid JPEG formats.
976ca8005 Use InetNtop on Win32 rather than WSAAddressToString. (#170)
95e529566 Use InetNtop on Win32 rather than WSAAddressToString. (#170)
4800c201e Add Axis camera creation functions and Http camera NT publishing. (#420)
dc9a9e5d9 Fixes typo in SensorBase error message (#422)
4b16999fb Fix tests that are only built on cmake. (#168)
318d23ba1 Add AxisCamera wrapper class.
9c4c7c08b Java HttpCamera: Make accessor functions public.
9016a9e8b Start implementing HttpCamera.
4c8c41fdc CvSink: Support grayscale images.
205d3b1d0 Some cameras don't provide the JFIF header, so don't require it.
1575fff07 MjpegServer: Limit the number of simultaneous clients to 10.
8c7338f2b TCPAcceptor: Include port number in error messages. (#166)
459cc65b3 TCPAcceptor: Include port number in error messages. (#166)
6844f05c3 Adds SetAllSolenoids function to CTRE PCM code (#419)
8f67f2c24 Remove unused code and add pmd check (#395)
cc246bb9a Switches JNI to use new jni_util methods (#404)
3d2827567 UsbCamera: Simplify the message passing implementation.
bf9f0a9e6 Add vision pipeline API to make it easier to run OpenCV pipelines (#388)
6f41b3cde UsbCamera: Implement LifeCam HD-3000 exposure quirk.
9ffc09a11 Fix settings example if there's no "--" in arg list.
ced2608af Move ExtCtrlIoctl to UsbCameraProperty::DeviceQuery.
adbca532c Break up UsbCameraImpl::DeviceProcessCommands().
5e38d8f28 Refactor UsbCameraProperty.
d6ef2c04a Start refactoring source property implementations.
ce6978387 UsbCamera: Scale some properties to make them constently percentages.
c3160bad4 Update SendableChooser to use HashMap (#394)
94b8ac42c Adds CPP version of vision pipeline (#399)
a03e3d7eb Fixes HAL to build without all headers included in HAL.h (#418)
a42ec08d1 Fix RoboRIO Cross-Toolchain GCC Status (#405)
fddb6cd9d Fixes SPI Accumulator averages (#415)
10b13da3d Refactor USB vendor/product to product string to UsbUtil.h.
88afefe46 Avoid crash in SourceImpl constructor.
841f01601 Fixes SerialHelper when used with OS serial ports. (#411)
8e4afc95c Switches C++ strings to StringRef (#336)
80abf6bf2 Support per-stream resolution settings.
94359709a Add new ARM Toolchain, add toolchainpath to README (#165)
8cec94869 Fix typos (#409)
31ae7c942 Fixed signatures of deleted assignment operators (#408)
d0900626d Removed format.py shim (#393)
0ee4cadca CameraServer: Workaround LabVIEW dashboard OpenCV compatibility. (#406)
8501b7c9e CvSourceImpl::CreateProperty(): Use lock_guard instead of unique_lock.
0ce0855a6 Java: check for null and use new jni_util features.
e1dabbc2d cscore_cpp.cpp: Fix include order.
c08a489e2 Improve error message for VIDIOC_STREAMON "no space left on device".
28a2ba4bf Add external logger interface.
2b8b8e740 Improve logging consistency by using source/sink name throughout.
bdaf60b2d Replace tabs with spaces in gradle files.
7c2f994a6 Refactor source IsConnected() and connected notification.
24b5a9c38 Adds UsageReporting include when using the LabVIEW shim (#401)
bb9f5b749 jni_util: Add JException wrapper class.
c091d74de jni_util: Add JException wrapper class.
626220e9f Fix javadoc typo (#403)
19be09c36 Add null checks on all JNI object parameters.
57d053a8f jni_util: Make JStringRef and JArrayRef null-safe.
8c2a148ed jni_util: Make JStringRef and JArrayRef null-safe.
0e43765c5 jni_util: Add GetJavaStackTrace.
6fbaf57b9 jni_util: Add GetJavaStackTrace.
12aee3e02 Make JLocal and JArrayRef movable but non-copyable.
2df00647d Make JLocal and JArrayRef movable but non-copyable.
498a8e2b7 Fix JArrayRef DirectBuffer destructor.
99395273c Fix JArrayRef DirectBuffer destructor.
4568156bd Make JClass more useful and use it in ntcore JNI.
ce7611562 Make JClass more useful and use it in ntcore JNI.
323d68d46 Fixes multiple initialization of HAL_BaseInitialize (#400)
db2091dd9 Adds safe serial port write methods (#396)
8216d85e5 Adds check for PWM overallocation (#392)
a705eb1c6 Add PMD and solve issues (#389)
77f664a6b Fixes simulator HAL builds (#391)
c30057e92 Remove unused imports (#387)
81fd0eefa usbcvstream example: Print error from GrabFrame.
b5fd15e05 CvSink::GrabFrame(): Add delays like MjpegServer to avoid consuming CPU.
ecfc68417 MjpegServer: Increase bad frame wait from 10 to 20 ms.
4600ea135 Frame: Make Data struct public.
3b82ba894 Refactor JPEG functions to JpegUtil.
c32fc57ce Refactor HTTP utilities.
9a8f66e3e SourceImpl: Add no-copy PutFrame.
ef3971321 Revert "Don't output \r\n before boundary.  This throws off some clients."
da68fea08 Fixes shared wpiutil switch on athena (#24)
06108b947 Update CameraServer support. (#386)
1f93a4ab4 Change acronym class names to use MixedCase (UsbCamera, MjpegServer). (#22)
7a587390b Fixes OpenCV on windows when we get to it. (#23)
16181523c Put language in version string (#377)
7e0706cf8 Updated Usage Reporting for 2017 v8 Image (#385)
bff67887f Adds capabilities to support certain HAL functionality from LabVIEW (#382)
351ff5eb4 Adds new Threads API for priority setting while using std::thread (#379)
378a145cf Fixes Missed Symbol from Def file (#162)
3625f11e0 Adds C methods that take a cv::Mat* (#21)
051df1404 Updates ni patch task to automatically switch all files to LF (#381)
ee0318f69 Updates NI Libraries to image 8 (#380)
1332ba3ad Adds JNI symbol check to ensure we don't miss any definitions (#19)
558b2ffa4 NetworkStream: Add setBlocking() and getNativeHandle(). (#161)
1315a3967 NetworkStream: Add setBlocking() and getNativeHandle(). (#161)
a6fb1efcb Switches internal joystick errors to take StringRef (#378)
31fecc28a Check for NaN motor speed. Fixes artf5467. (#376)
00b76d42e Fixed unused variable warning from gcc. (#159)
e7c4150c0 Fixed unused variable warning from gcc. (#159)
bc06c843c Adds JNI symbol check to ensure we don't miss any definitions (#160)
30b1efc35 Adds check to ensure all JNI symbols have been defined properly (#373)
7d721eb56 Fixes memory leak in SerialHelper VISA (#366)
b8e9439d3 Message: Fix typo in error message. (#158)
d48aac5be Gradle Update (#372)
14b56db99 Gradle 3.2.1 (#369)
bfe429de5 Switches handles back to std::array (#367)
57ef5cfd0 Splits out SerialHelper to allow using OS or VISA resources (#365)
4bbb7c0bc Fixed javadoc generation to actually download cscore and ntcore and include the source in its output. (#368)
4de70bff5 Adds missing JNI method definitions (#371)
687f0c7dc Use const parameters in std::sort lambda. (#364)
f3d66e92e Searches for USB serial devices to get the correct VISA object (#363)
046d385a7 Actually fix lack of - (#157)
5caf75237 Fixed version generation when no - is present (#156)
9a91ae54d Fixes incorrectly changed Java enums (#360)
69422dc06 Replaced floats with doubles (#355)
7bcd243ec Reorder the HAL handle types (#357)
8f70bea89 Fixes Athena Runtime build not including ntcore (#356)
948c4275a Fixes the wpilibj jar not getting updated during every build (#359)
03a115991 Java CameraServer: Fix type cast exception. (#358)
a19b1b934 Adds data accessor functions to JNI (#18)
1c8fb298b Ran formatter (#354)
b25a7cb37 Command::IsFinished() must be overriden by subclasses again (#353)
140c365e4 Added XboxController class (#140)
8c93ceb72 Add cscore-based CameraServer. (#352)
0b1e876dc Bump wpiutil version to 1.0.2.
ac50d7cf8 Removes NiVision from allwpilib (#305)
ae8c8ec23 Don't output \r\n before boundary.  This throws off some clients.
ecb873ff4 Fixes myRobotJava zipping before Java gets built (#351)
ec8c0eb3c Fix FPS setting and crash in reapplying settings.
3d898dd8f MJPEGServer: Fix URI %xx-decoding.
df18e178e MJPEGServer: Provide a bare-bones HTML root page.
71a6e0898 MJPEGServer: Make a couple of tweaks for Axis camera compatibility.
a05636d9a Add data accessors for USBCamera and MJPEGServer.
5eecbfd9b Implement immediate notify on network interfaces event.
78baa04ec Adds OpenCV to myRobotJava (#350)
65514b302 USBCamera: Update description on reconnect.
881d55f85 VideoListener: Add move assignment operator.
b78f580d4 Adds default methods for Commands (#238) (#238)
cc20d9d0f Add no-parameter constructors for specific sources/sinks.
b115c7522 Adds SPI DIO to WPILib (#256)
6bc092f3a Updates SPI device pin (#348)
f99a266ce Adds CsCore dependency to myRobotCpp (#349)
35aa54441 VideoEvent: Make sourceHandle and sinkHandle public.
bad4ca466 Add event for network interfaces change.
5fecc57e8 Add methods to get the hostname and network interfaces.
e1f4e3d2d Fix mapping into Java enums.
5ace9e418 Renumber event kind values.
9945459a4 Notify sink enable and disable events.
22c11fad3 Notify source property events.
c6b527d45 Notify source connect, disconnect, and video mode changes.
8e9911d33 Notify on sink source change.
b24572594 Notify on source and sink create and destroy.
154ae5dcb Notifier: Take Impl rather than handle in notification functions.
bae203708 Handle: Add methods to get Data struct from Impl pointer.
c0bc8d702 Change examples to start at port 8081 rather than 8080.
736b5ff42 Java: throw VideoException on error.
29cd2b11b enum_usb example: output the property type.
dcf773c3e Add USBCamera setting functions similar to the old WPILib nivision ones.
791cabbc2 Rename Type to Kind.
3c7d8063f VideoProperty: rename type() to getType().
3381340eb Add method to get source/sink type.
6446b9ef1 Add sink source changed event.
6c19eb59b VideoSource, VideoSink: Provide equality operation.
797d049f3 Use java.util.function.Consumer for event listener.
0bcafedeb MJPEGServerImpl: Refactor NeedsDHT.
7e0e8286e SourceImpl: Refactor frame allocation into AllocFrame.
5ae116237 Move MJPEG DHT insertion from SourceImpl to MJPEGServerImpl.
c80c4ae55 MJPEGServer: Use worker thread model.
254b88bdb Start adding support for non-MJPEG frame types.
2657d8917 Adds a c_str method to StringRef (#155)
38ec59f03 Adds a c_str method to StringRef (#155)
1f724d58e Made the test scripts executable to fix -dirty on the version numbers. (#347)
259cf1ff3 Mark NetworkTablesJNI callback interfaces as FunctionalInterface. (#154)
de4a246bb Adds OpenCV Loading to wpilib program startup (#338)
b12658afc Make SafeThreadOwner moveable. (#153)
c23880f82 Make SafeThreadOwner moveable. (#153)
41dd9e4f0 Loads OpenCV JNI Library in Java (#15)
412b80da3 Fixes joystick button handling HAL JNI layer (buttons are in an (#341)
47319960a Fixes a missed change to unique_ptr in the HAL (#337)
b22d21823 Removes CANJaguar specific constants from JNI files (#339)
3d1f69075 Adds a size parameter to JStringRef (#152)
dc94a3fac Adds a size parameter to JStringRef (#152)
6eb82bc31 Exclude genlinks.bat from HAL zip (#333)
a06dd25d5 Adds cscore and opencv to wpilib (#332)
468cac543 Fixes JNI
d4b48216e Renames uberzip to athena-uberzip, and doesn't include static library in it
e1bb05bc5 Updates to the 2017 v7 image (#329)
f83ff41e4 Rename from cameraserver to cscore.
891ce0631 Adds opencv headers to uberzip (#11)
0658ba6f7 Adds javadocs and sources to published windows builds (#10)
29d8d1d74 Implement listener interfaces.
8d2efb283 Improve listener interface.
e07a40a16 Adds an UberZip that preincludes the opencv libraries
38a3eda6a Switches CameraServer on Arm to use non-included native library
6d1ab7606 Enables temporary publishing on windows
8694a020a Removed entries from .styleguide that are included in .gitignore (#331)
1efb2e4d3 Deprecates Task and Semaphore, and changes other deprecations to use wpiutil version (#330)
77edf1e10 Add FRC Driver Station connection support.
28c8678ea SafeThread: Use atomic for m_active.
34acd9d47 SafeThread: Use atomic for m_active.
861726cef Adds gradle task to fix NI libraries whenever new ones are added. (#325)
81212d80c Fixes ultrasonic declaration (#328)
2235a809c Fixes a missed change from the ni-libraries location change (#326)
3fe0f49ac Moves NI headers to their own folder in the repo (#324)
03d8f9193 Moves using namespace from ChipObject.h into hal namespace (#323)
05ca76ea9 Runs clang-format on ntcore (#150)
d1065f0bd Moves deprecation definition to wpiutil (#149)
cf0ec7b9a Moves deprecation definition to wpiutil (#149)
5e54969f6 Adds Task back into WPILibC (#320)
46085824a Add DHT data to MJPEG source images if not present.
18ef5eb1b C++ and Java tests for the new build system (#309)
b5e3d92e9 Remove errant "using namespace" in global namespace (#318)
ba8761e39 "using" directives are no longer used in global namespaces (#219)
78f0b1562 Removes Task.h from wpilib.h header (#317)
5c28b3002 Removes NetworkTables from Java executable jar (#303)
59267da72 Namespace all wpilibc functions/classes into "frc" namespace. (#311)
36ad45c07 Delete .gitreview (#316)
ecb2add79 Removes additional references to CANTalon from the library. (#312)
9bbdaf300 Removes the task class from wpilib (#314)
bc492bb40 Removes the REAL definition from wpilib.h (#315)
7067179b2 Ignores the vscode directory. (#6)
81221c487 Moves JNI internal utility methods to frc namespace (#310)
4828a6986 Update versioning plugin.
3fcc808e9 Use zlib built into OpenCV.
15cdd661a Simplify OpenCV dependencies.
137b3d81f Fixes JNI file not being copied to output directory for integration tests (#308)
6741f47ef Update version plugin (#307)
60d9f3de6 Update version plugin (#148)
f87baaa4f Moves dependencies to their own gradle file, and adds capabilities to link to ntcore and wpilib (#4)
fecd8a448 Fix wpiutil version (#147)
fe4ef75cf Updated wpiutil version
87c7a9db5 Add USB and OpenCV processed dual stream example.
fa2ce4008 Add OpenCV library links to example executables.
4eac3fe9a Disable x86 platform for now until we get OpenCV working with it.
d7efd6251 build.gradle: Fix OpenCV library order.
e10b399f5 CvSource: Implement NotifyError.
39a819538 CvSourceImpl: Add stub for CreateProperty(callback).
e75e9092a Fixes athena runtime zip not being built at the correct times (#306)
247cef5ec Removes CANJaguar from wpilib (#300)
43331419f MJPEGServer: Output boundary at start of image instead of end.
c66a55d81 Implement CvSink.
9a44a3814 Fix source and sink refcounting.
5f69cb2a5 SinkImpl: Refactor GetDescription, add GetError, SetEnabled.
a5f63c3ae SourceImpl::GetNextFrame(): Avoid spurious wakeups.
005396218 SourceImpl: add PutError().
c462d0b24 CvSourceImpl.h: Remove unnecessary include.
29f999e2b Adds new handle structure and type for vendors (#297)
778edaeb2 Reverse the JNI load order to look for system library first. (#145)
273a395a2 Client: when reconnecting, default to NT3. (#143)
5e3755493 Linking and maven publish location rewrite (#298)
aa49ebd47 Links wpiutil on roboRIO dynamically (#141)
c45384b91 Removes the native library from the RoboRIO Jar (#139)
70343c0b3 Fixes missing function for Timer::GetMatchTime() (#299)
88fdbc6d3 Readds wpiutilZip dependency to the build. (#144)
7ca6c5ef3 Remove RemoveSourceProperty.
60e199b0b Fixes format.py on windows (#293)
1071686d8 Replaces C++ IsNewControlData semaphore with atomic bool, and Java code with AtomicBool (#187)
511d55154 Implement most CvSource functionality.
fdebdd520 Added format.py to Travis config (#236)
7ea13f7e0 Refactor properties from USBCameraImpl to SourceImpl.
5ca5583fc Removed unnecessary set of parentheses and ran formatter (#290)
75463a249 Implements threaded notifiers and interrupts in the HAL (#281)
7280d241f Fixes DIO not erroring with too high of DIO number (#288)
963391cf3 Cleanup SolenoidBase, Solenoid, and DoubleSolenoid (#271)
4f4c52d6d Switches JNI to use SafeThread from WPIUtil (#282)
499da6d08 Updated the wpilib version plugin to 1.2. Fixes issues with submodules. (#289)
6754703ad Updated the wpilib version plugin to 1.2.
89b8e5435 Updated the wpilib version plugin to 1.2. Fixes issues with submodules. (#138)
b0ab351f7 Don't check for existence in distributing .debug files. (#137)
39e4d11f8 Remove .pullapprove.yml (#287)
9b6f4ecd0 Don't check for existence in distributing .debug files.
df4a2c07f Checks for system initialization in functions that can be called before creating handle based objects (#285)
c46c49037 Adds wpiUtil to HAL and JNI (#280)
9142cbb82 CvSource: Update interface functions.
1f6b38632 Implement frame timestamps and use wpi::Now() for generation.
f2751db5c Adds NI Libraries symbolic link (#286)
aad1266a9 Distribute shared library .debug files.
06a40680a Change debug strip to be part of link task (as doLast).
cc2cbf810 Exclude opencv MANIFEST.MF files to prevent repeat unzips.
cf9aa9032 Don't overwrite platform-dependent opencv zip files.
d51f6c45e JNI: initialize status to 0.
017ec83ce Add support for OpenCV to Java wrappers.
6641612de Update to use wpi version plugin.
218718a06 Add OpenCV dependency and update OpenCV interfaces.
353041535 Distribute shared library .debug files. (#136)
e4234f519 raw_istream: Add std::istream style in_avail(). (#135)
d81840d6c raw_istream: Add std::istream style in_avail(). (#135)
1d336996b Dispatcher: Minimize amount of time m_flush_mutex is held. (#132)
86c43df8d Fix connection notification races. (#130)
5c1b7ecd1 Change debug strip to be part of link task (as doLast). (#134)
011ac1fa2 Java: Allow any Number type (not just Double) to be passed to putValue(). (#129)
2bff276ad Fixes a missed formatting issue (#283)
e5e1a1a4d Moved version generation to the WPILib versioning plugin. (#277)
b8e5258cf Creates the build dir if it doesn't exist, and deletes the version files during clean' (#131)
56179088b Updated the gradle build to get the version from the WPILibVersioning plugin (#123)
780e9580b Ignores KeepAlive messages during initial handshakes (#128)
0613f1d18 Cleanup encoder class (#272)
8b94e0933 Update README (#261)
23ef57561 Cleanup PowerDistributionPanel (#275)
33b95816e Cleanup SensorBase class (#273)
4c1e4fd80 Updates image to v6 (#278)
b775b01e0 Optimizes NetworkTables string concatenation (#125)
0a8e0e974 MJPEGServer: Pass resolution and FPS requests to source.
89805a44c Fix NetworkTable::setFlag typo (#124)
9caa0af4d USBCamera, HTTPCamera: Name functions consistent with classes.
70531762b usbstream: Wait for enter rather than infinite loop/sleep.
30f4ecd17 Rename HTTPSink to MJPEGServer.
d56c3f9ad USBCamera: Default to MJPEG and lowest resolution.
73a97c177 USBCamera: Finish implementing mode support.
3784b665d Updated version for beta 1 (#270)
567ea1d58 Remove CAN TalonSRX from WPILib (moving to external library) (#268)
55346e28d DoubleSolenoid free reverse port handle instead of channel (#274)
8ec2b1d96 README: Fix artifact names.
c858e0391 README: Remove reference to cmake.
63c9af457 Add license.
473a87a76 USBCameraImpl.cpp: Clean up formatting a bit.
4c6f6536b tests.gradle: Link to wpiutil.
cb4d8a655 Add examples.
417545d52 Finish most of USBCameraImpl.
7f88bd15d SinkImpl: Keep enabled count rather than just boolean.
49b8c4ba7 Fixes the gradle dependencies for building (#269)
f1c2b6656 Reverts the 2017 Image for Beta 1 (#264)
27bf94fd0 Remove kDefaultPeriod from IterativeRobot (#232)
4896a77c8 Fixes guarantees for waitForData (#252)
d1d3f049f Solve scp file not found (#263)
7b3f6eeae Add raw_fd_istream. (#121)
f6df7cad9 Add raw_fd_istream. (#121)
15cb50516 TCPStream: Avoid SIGPIPE signals.
cc1b94afd TCPStream: Avoid SIGPIPE signals.
c313dde03 Updated the rpath to point to the correct location for the Java integration tests. (#262)
8d1c51b7e Update image 2017 v5 (#254)
a59e00e90 Adds a test to make sure WaitForData is properly waiting (#258)
7070162b9 Fixed lint errors (#259)
9859c14ca Fixes status parameter in wrong location for initialization range functions (#260)
a7eca7d4b Adds ConnectionInfo to Rpc callback (#116)
64ebe7f5e Updates SmartDashboard with new NetworkTables3 functions (#162)
8b2345a70 Removes an unnecessary function from HAL Power (#237)
e65f9908d Makes IterativeRobot not double check for new control data (#253)
fd52912d7 Fixed return value propagation of format.py (#251)
9e7993905 Remove slack integration (#257)
9047c98e6 Fixes possible indefinite timeout on multiple RPC calls (#120)
a65620722 Forces exceptions to throw on HAL handle creation functions (#209)
53d078966 Storage: Escape equal sign in strings. (#119)
ee24a6f4f Disable support for non-MJPEG formats.
70616c48e Add support for enumerating and changing USB camera video mode.
81e63ea3a Fix simulation build with latest ntcore/wpiutil. (#250)
d4bbd5cc6 Make raw_socket_istream constructor explicit.
1affae956 Make raw_socket_istream constructor explicit.
7463e0208 Logger: Use raw_ostream and SmallString.
d3ed26f7c Logger: Use raw_ostream and SmallString.
f711ced4c Add raw_socket_ostream.
1ec89fc4f Add raw_socket_ostream.
a92b7298f Add 1-character read to raw_istream.
de07b01a7 Add 1-character read to raw_istream.
4c6c096c5 Change API of raw_istream to be more similar to raw_ostream.
c2ae897b0 Change API of raw_istream to be more similar to raw_ostream.
b2e129197 Refactor JNI helpers into wpiutil (support/jni_util.h). (#105)
94c2b6579 Refactor JNI helpers into wpiutil (support/jni_util.h). (#105)
760d6a26d Make SafeThread header-only. (#117)
3a419768c Make SafeThread header-only. (#117)
d8ee44349 Update to latest LLVM code (#88)
ee4244850 Update to latest LLVM code (#88)
d90cf843e Move common utility classes to wpiutil library. (#79)
f6b700ea9 Move common utility classes to wpiutil library. (#79)
5dfae8d6b Fixed include order (#245)
107a4cc1e Add wpiutil dependency. (#190)
35d51d68f Made a comment use more professional language (#249)
049fec470 Fixed compilation with GCC 6 (#248)
659dbef75 Ran format.py after writing unit tests for and fixing bugs in it (#239)
a5fe605aa HTTPSink: Add basic property setting support.
7818c3bda USBCamera: Normalize property names.
d5e5755ff Collapse boolean/double/enum properties into just integer.
8fbc23b1f HTTPSinkImpl fixes.
ac9b6f7b1 Implemented CircularBuffer resizing (#224)
425ed464e Removed leading underscores from variable names (#246)
2c15bb247 Remove delay from periodic methods. (#243)
28e178b1a Update Compressor documentation (#244)
d05f0820b HTTPSinkImpl: Report property step and default.
dad44cc92 Add status parameter to property functions.
2c80587d1 Start implementing USBCamera.
80eb05643 Fix property handles (and allocate more bits to the property index).
80e546b79 Adds a way to externally test C structures (#115)
6eba04ed8 Use copy-and-swap idiom for reference-counted classes.
c606671d2 Consistently name property max/min functions and add step/default.
9bb37d5df Remove support for multiple channels.
5d2a08443 Fix Typos. Fixes artf4853 (#242)
e952236e1 Reverts the last 2 Rpc changes (#114)
9a3100b22 Fully asigns the ConnectionInfo struct (#113)
7e9754acf Passes the ConnectionInfo of the Rpc client on server callback (#112)
087eeec76 C standard library functions and types are now prefixed with std:: (#227)
dbe03afb9 Fixes error in driver station control word cache (#222)
2ecb939b3 Add a method to detect the HAL runtime version (#228)
1416fb8f6 Update Periodic JavaDoc (#231)
aeb6c4889 Added license template file (#225)
c2fc6711d Switches enums to use a fixed size for their base (#230)
f271185cb Adds a RobotPeriodic method to IterativeRobot (#226)
b78592d62 Log.h: Remove unnecessary includes.
052f746c6 Use StringRef and ArrayRef return values when buf passed.
c4ceec145 Enable unit tests.
775386d8b HTTPSinkImpl: Implement GetDescription.
451c08ef7 Add wpiutil library dependency to builds.
52c8743b3 Finish rename of EnumerateSinks to EnumerateSinkHandles.
5c59b9aeb Make HTTPCamera constructor public.
3e00dabd1 Import gmock.
4fc7daedc Fixes incorrect deadband value being passed over PWM (#223)
c5c069743 Switches AppVeyor to use same build for 32 and 64 bit java tests (#107)
ba241cd7f Source: Keep track of how many sinks are connected and enabled.
ddb97bfaf Fix SendStream error handling.
e415ca66b Initial HTTPSink implementation.  Untested.
85be299da Start implementation.
9dd5bea7a Create a common CS_Handle.  Add initial status values.
e71abedef Add logging implementation.
3888d7726 Adds connection listeners that can be called statically (#111)
6bfc00867 Replaced snake case variable names with mixed case (#221)
2c94d0ba2 Cleaned up integer type usage in the HAL (#192)
0cd05d1a4 Cleaned up integer type usage in wpilibc (#92)
ff93050b3 Remove static_assert for sizeof(uint32_t) <= sizeof(void*) (#220)
05626cfaf Fixed cpplint.py warnings (#215)
59ec54887 Switches notifier to use a typedef for the callback, and prepends HAL_ to InterruptHandlerFunction (#216)
028efff50 Ran format.py (#217)
fc48944b4 Fixes Rpc Cancel. Check was only in timeout case instead of global (#110)
2499771cf Added extensions to .styleguide (#214)
c989ae808 Updated gradle to 3.0 (#208)
2150f5879 Adds AppVeyor support for VS 2013 and 2015 (#106)
85156d15c Fixes the final issue with VS 2013 builds (#109)
6943d14f9 Java: Use wpiutil jni-util.h helpers.
ade4e87d6 Fixes support for MSVC 2013 (#104)
8007a7b15 Correctly handle UTF8 Java translation for StringArray. (#103)
e8643600f Implement all current JNI native functions.
cb7f1f6e3 Initial .def file real content.
af7132be8 Initial CameraServerJNI.cpp commit.
ec080118f Clean up odd special case of GetSourceProperty().
6bcc0e2d8 Correctly prefix CS_EnumerateSourceProperties.
2fd81a7e3 Add copyright notices to Java files.
2f99f81aa Java: Add VideoProperty implementation.
97f1f1c9c CameraServerJNI: Fix case of a few functions.
0fbb2e8a1 Add methods to get property name and enumerate properties.
0158fd35f Continue implementing C wrappers.
80b15b7fe Add a bit of description about how handles work in the C API.
2acca6eeb cameraserver_cpp.h: Move stdint.h above the C++ includes.
151c89fb5 Rename FrameGrab to GrabFrame.
4f22ac410 Move all non-trivial inline functions to cameraserver_oo.inl.
7c1da2dfc Split classes in cameraserver_cpp.h out to cameraserver_oo.h.
b5d32ec84 VideoSource, VideoSink: Move the status flag too.
aec16a934 Revamp API again and start implementing C and Java wrapper shells.
93b486b6b Replaced C-style casts found by GCC in HAL, wpilibc, and JNI (#211)
075155b43 More gradle 3.0 additions (#102)
c25c62e0a Update gradle to 3.0.
2ec6132fc Switches compiler from -O0 to -Og (#197)
8aba2b285 Adds .vscode to styleguide ignore files (#210)
32c95fa0d Moved style guide documents and scripts to wpilibsuite/styleguide (#207)
e653a228f Updates gradle to 3.0. Now that the check bug is fixed, removes the empty check tasks. (#101)
b2831347b Revamp API.
7845caa10 Fixes GetRpcResult not removing calluid from call list on non blocking call (#100)
66d214c8a Finishes blocking call canceling (#99)
277cf2a08 Removes new line from mdns name (#98)
7bf44e951 Fixes server side polled rpcs (#96)
c0ce4270f Adds a way to cancel a blocking rpc call (#94)
d6e8de21e Fixes rpc timeouts and blocking call list error (#95)
1635cba82 Reduced minimum update rate from 100 ms to 10 ms. (#89)
a78647062 Only allows 1 blocking call per Rpc Call Id (#93)
4164e670d Adds Visual Studio Code files to gitignore (#92)
062470ef6 Add llvm::SmallSet. (#90)
0b80bd2b0 Adds timeout capabilities to blocking Rpc Calls (#86)
40365faed Adds a static mutex to notifiers to stop destructor race condition (#204)
e1515299c Initial commit.
776cb915b Revert "PIDController queue now stores inputs instead of errors (#138)" (#205)
5e9fe2f5c Adds an ignore for Visual Studio Code files (#203)
7501ae65a PIDController queue now stores inputs instead of errors (#138)
63469c00c run-tests-on-robot.sh: Save coredump on crash. (#201)
3df257cdb Set duplicate strategy for all jars and zips to prevent duplicates from occurring (#191)
45b8e9ab4 Renamed "pin" to "channel" and variables with underscores now use mixed case (#194)
227fdc1a6 Updated C++ style guide (#196)
d347cebf6 Cleaned up odd formatting from static_cast by using "u" integer literal (#200)
3819cd076 Updated cpplint.py and fixed its regexes for C-style casts (#193)
e8f1fdda4 format.py now emits warnings for modified generated files (#195)
fd4719cb8 Fixed Doxygen comments for LinearDigitalFilter (#198)
bc99d341f Changes remote_name to remote_ip (#87)
6ef4745d8 Added Joystick::GetAxisType() (#98)
0f9f7309e Sets the duplicate strategy to exclude in all zip tasks (#85)
e6244289f Create dummy wpiutil library. (#84)
30fbfe46e Create dummy wpiutil library. (#84)
a73166a66 Make many more utility classes/headers public. (#76)
62a980d3c Fixes Relay Constant Test (#189)
866046edd Some general cleanups in the HAL (#188)
8ac7e44f1 Updates gradle to 2.14.1 (#186)
d66f65e37 Updates gradle to 2.14.1 (#83)
a831978cc Uses the fixed SensorBase functionality in the WPILib (#185)
8da577b56 Moves FRCDriverStation to athena folder (#184)
512ecf649 Makes SensorBase checks use HAL check methods (#182)
0901ae080 Switches the HAL structs to use unique_ptr (#183)
1ca291f20 Fixes a missed HAL_Bool change from int32 (#181)
75eabfee1 Moves DriverStation HAL code to its own header and definition (#179)
f7c3f13a7 Improve CircularBuffer documentation (#180)
57efd13f7 Replaced PIDController::Initialize() with delegating constructors (#178)
7ddc15362 Fixes analog gyro casting to float then returning double (#177)
f9ebd3bde Fixed PIDController::GetError() for continuous inputs (#169)
2c911b0f7 Adds a maximum time based cache to HALControlWord data (#158)
1bf2d58db Reordered DriverStation functions in wpilibc and wpilibj to match wpilibc header (#166)
c93b5bedf Miscellaneous cleanups for wpilibc PID controller (#175)
dffaa0abb Moves HAL cpp headers from root to HAL directory (#174)
20c6525b1 Cleaned up wpilibj enums (#167)
fe7165a8f Removed double-add of shared classpath. Fixes #157. (#164)
d2aa168f6 Implements Better Error Messages from the HAL (#172)
05c00430b Fixes CPP lint errors added to HAL. (#173)
530508716 Fixes a missed float to double change in the HAL. (#176)
b979fc2a6 Adds SetDefault Java definitions to Def file (#82)
58092c519 Adds SetDefault methods to NetworkTables (#54)
6615a34e9 Added contributing and license files (#63)
50a261283 Notifier: maintain freelist to reuse uids. (#81)
6251697f6 Added Lint task and parallelized format.py (#161)
ea1a1e6bc Makes the CANJaguar error status messages more useful (#165)
edf5ecd4a Changes HAL Headers to be C compliant. (#171)
1b1ee7f9f Renamed spiGetSemaphore() to spiGetMutex() (#170)
3fade171f Fixed inconsistencies in documentation (#168)
c99e89dfc Fixed the remaining cpplint.py warnings (#160)
b51e85ae2 Switches HAL to fixed length signed integers, and adds our own HAL_Bool Type (#155)
4a98e6881 Moves the DS new data wait calls down to the HAL. (#156)
0cb288ffb Fixes warnings thrown by cpplint.py (#154)
e44a6e227 Refactored wpilibj HAL JNI to simplify generating it from HAL headers (#109)
aafca4ed7 Reduced duplication between formatting scripts with Task base class (#80)
ea6876e81 Some general HAL cleanups (#153)
aa9c2b2c9 Made Log.h use std::chrono (#136)
43a2eccdc Finishes Prefix Renames (#152)
b637b9ee4 Prepends all HAL functions with HAL_ (#146)
5ad28d58e Switches PWMs to do scaling at the HAL level. (#143)
be2647d44 Switches Java to use HAL Constants (#145)
4a3e3a632 Changes DigitalSource API for HAL ease of use (#144)
7597e3c27 Switches handle resources to dynamic arrays (#142)
0a983eeeb Moves Gyros to the HAL (#131)
b036bf2e3 Add method to get the port number of a Joystick (#137)
73602e6cb Added missing include for robot startup macro (#135)
72455280a Removes unneeded resource includes from WPILibC files (#134)
ea95bb85a Adds Constants from new constants class to encoders (#133)
fb865d3ee Adds a special exception and status message for a handle error (#127)
36ac37db8 Moves Encoders to Handles and Moves WPILib Encoders to HAL (#124)
b45e0917a Adds port constants to the HAL (#130)
0e127679c Removes some unused variables from DigitalInternal (#128)
2f36d508c Gradle 2.14 (#118)
cf29d4560 Moves HAL PCM objects to header and HAL namespace (#129)
8c4ad6242 Switches Solenoids to Handles (#126)
62c217cd0 Switches compressor to handles (#125)
0f105a26f Switches Counters to Handles (#123)
eb4350033 Update Travis OS X build to use newest image. (#75)
47694ef81 Switches DigitalPWM to Handles (#121)
f77a976fb Switches resource errors to AllocationExceptions in JNI (#122)
3593ecb17 Switches PWM and DIO to Handles (#120)
384ad57d2 Updates Gradle to 2.14 (#78)
9b2af0d09 Switches relays to handles (#119)
e8e052712 Switches AnalogInputs and AnalogTriggers to Handles (#117)
77a1af44c Removes freePort from the HAL (#116)
5e2a76147 Moves handle definitions to main handle header (#115)
e1d4845cc Move Analog Outputs to Handles (#112)
95d40ed01 Fixed issue with digital outputs used as pwm on mxp (#14)
aa22d4c33 Clarified that PID controller runs in discrete time (#107)
046e043c4 Moves Interrupts over to Handles instead of pointers (#99)
74fc10999 Fixes preferences formatting (#108)
cee9b2609 Added std:: prefix to more C standard library uses (#106)
daa0260a4 Deduplicated UsageReporting definitions (#104)
d66c61a36 Cleaned up robot startup and cleanup/shutdown code (#77)
ecc210f99 Rename Notifier::m_handlerMutex to Notifier::m_notifyMutex (#105)
3cacc2aba Switches indexed handles to shared_ptr (#101)
085c47b67 Unbreak wpilibc WritePreferencesToFile test. (#103)
4b516de18 Don't delete persistent entries in DeleteAllEntries. (#71)
c7d9ecbab Fix mac build by not defining false and true. (#73)
039515082 Fixes formatting in error.cpp (#102)
c3908660b Switches all HAL Handle errors to be zero (#100)
b2795af2b Added note that multilib GCC is required when building the native version on 64 bit Linux (#66)
6c6c087e3 Fixes .gitignore ignoring needed files (#68)
3c77faaf6 Fix UTF8 conversion in Java. (#70)
fc515f457 Changes HAL Port from a pointer to a handle (#93)
5a82f73d9 Replaced .h C headers with c-prefixed version and added std:: prefix to C standard library usage (#90)
776a991d6 Moves Notifier over to handles (#97)
8527f2c2a Fixes HandleResource classes (#95)
c85b72b95 Fixes a few bugs in the initial handle implementation (#94)
f0d9e19b5 ErrorBase: change to ostringstream. (#66)
c7c011813 Made include ordering/organization more explicit in style guide (#86)
d76f0d402 Adds support for Handles to the HAL (#91)
de7678f2f remove author requirement for now (#84)
2f7eff709 Removed @author from comments (#88)
7a402b417 Replaced "RoboRIO" with "roboRIO" in comments (#87)
10b4814e6 VisionAPI.cpp: Remove non-UTF8 characters. (#85)
613309c0a Replaced wpilibj class Init() methods with delegating constructors (#79)
c622c03ef Places while loop around DS wait condition. (#83)
ed7d2d6aa Splits the HAL Analog Implementation Files (#82)
51ff9e9f6 Classes which use I2C or SPI now do so via composition rather than inheritance (#72)
9f2f301fa Deduplicated FRC network communication headers (#71)
fa8bb3fa9 Remove obsolete timer functions and replace with std::chrono (#64)
4af0bbdde Add Travis Badge to ReadMe (#81)
4aca3c202 Removed extraneous curly braces around std::lock_guards in PIDController class (#78)
753ab85ef Removed commented out includes and imports (#76)
da6b8c7ae Split HAL Digital Implementation files (#59)
305ab08f1 Renamed DriverStation::IsSysBrownedOut() to DriverStation::IsBrownedOut() to match the Java version. This changes the external API. (#67)
e9718fc7b Replaced NULL with nullptr in C++ source files (#70)
8566878af Fixed C++ simulation build with Boost 1.60 (#49)
62812faf6 Clean up include guards and EOF newlines (#65)
d82635bbe Reordered headers according to the style guide (#58)
a598e2d09 format.py: add missing 'sys' import (#69)
b5b2f0c7d Fixed typo in LinearDigitalFilter documentation (#62)
e6f6d2478 Removed empty default constructors and destructors from Gazebo plugins (#63)
21b2aabf1 Removed unused DS_LOG() macro (#61)
74f9da9da artf2612: Added Python script for updating license in source files (#57)
e63830ed2 format.py: Fix running from styleguide directory on Windows. (#55)
eb51de65e format.py: Error out if no files found to format. (#56)
565a125da Reformat for recent commits (#54)
3af8e7e9f ControllerPower.cpp: Use quoted include instead of bracket for HAL. (#53)
e3eae023d Cleanup of gazebo plugins (#46)
6d9b3b0aa Splits HAL Digital and Analog Headers into multiple headers (#52)
9e99df1cf Removed sync group from SpeedController interface (#51)
e842ff7ad Explicitly stop the PIDController Notifier in the destructor (#48)
21b1e39b2 HAL Notifier: Use freelist to avoid use-after-free race. (#50)
8fc55c80a Renames all our .hpp HAL files to .h (#44)
248ca0c4a Added verbose flag to format.py (#47)
0d655d7ce Ran formatter again (#45)
e71f454b9 Renamed folders for consistency, using sim/athena/shared schema (#27)
54092378e Changes HAL to return -1 to 1 for joysticks (#35)
a4f0c4fbe Implements locking in C++ DriverStation, and adds double buffering to DS Thread (#25)
0965d60a7 Fixed a bug where using the two parameter constructor for CANTalon could cause invert-direction to not be initialized. (#26)
f2d601d83 Use interrupt() in our interrupt exceptions (#24)
e14e45da7 Add format script which invokes clang-format on the C++ source code (#41)
68690643d Fix travis configuration to include compiling sim (#28)
00b290210 Converts non hardware dependent tests to unit tests (#10)
975568c77 Adds Error Prone as compile time check to java projects (#13)
a834fff7b Applies Google Styleguide to Java parts of the library (#23)
64ab6e51f Create .pullapprove.yml (#42)
05a208981 Fixed PCM tests by raising tolerance slightly (#15)
6272244a7 Adds error-prone to check java code
a53251805 Adds all relevent tools to gitignore
320db8df1 Add .clang-format.
b827f484b Add Slack Notification from Travis (#11)
000a98f19 Fix apt-get update -q causing build to fail (#20)
bb64dd354 Updated README, and added in CONTRIBUTING. This adds tables of contents, as well as fixing a few other issues.
d1fb8cc20 Merge pull request #60 from wpilibsuite/build/appveyor
7d33059c2 Add appveyor badge to readme
d0c40b5ce Fixes indentation in .travis.yml
6794ff9ce Add Travis Build Configuration
ed0f197d1 Adds appveyor build config
25ad7a623 Merge pull request #59 from wpilibsuite/build/osxTravisBuild
5fb31baea Adds OSX Build to travis
ffb384ebf Add Travis Badge to ReadMe
95098c549 Add required apt packages to travis
386266842 Add Travis config file
083362bd8 Removed {@inheritDoc} from C++ sources and readded .inc files to Doxygen extension mapping
0f228e7b7 Implements a common PWMSpeedController base class
686f5d9fe Fix incorrect comments in the HAL I2C functions and incorrect return values for some I2C class member functions.
ad8763fc8 Removed unused pcre.h header. If regexes are needed in the future, use C++11's regex header.
1011b56b4 Fixes notifier restart error when last notifier is deleted
5904314a7 Improve ADXRS450 error detection.
e85254734 Makes Java Timer timing more accurate
fa8b68419 Make Java AnalogGyro.getAngle() thread-safe.
ec13d00b1 Fixed gradle build issue where files were not getting added to the wpilib java jar correctly.
36447d86d Merge "Clear error buffer total when clearing the buffer contents"
5ac68f74d Support client round robin to multiple server addresses.
b8ad1de33 Make members of ConnectionInfo and EntryInfo public.
1e14403b3 Update POV documentation with examples of POV angles
bee507eda Clear error buffer total when clearing the buffer contents
16343bbe7 Updated release to version 5.
623a5fcf8 Fix C++ CameraServer request handling.
6bd1654b8 Updated release number for the new release
952ebb11a This adds StopMotor() to the SpeedController interface for C++ and Java. For Java, this is as simple as just adding it, as all motors already have an implementation from MotorSafety that is correctly resolved. For C++, I had to override StopMotor in the classes that descend from SafePWM and explicitly call the SafePWM version. RobotDrive now calls StopMotor on each of its SpeedControllers, instead of calling Disable or setting the motor to 0.0 as it was doing previously.
91c5db06d Merge "More updates to the Gyro test fixing potential null pointer exception"
df33a7822 Added Config routine to allow enabling/disabling of limit switch and soft limits.  This improves upon the ConfigLimitMode routine, which does not allow certain combinations of enable/disabled limit features. Also keeps parity with LV and Java.
a33076ab9 Merge "Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically."
f80312b86 Merge pull request #52 from robotdotnet/master
836dc7a88 Fixes Connection Listeners
d567bd0bc Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically.
f436b33d7 More updates to the Gyro test fixing potential null pointer exception
728329388 Merge pull request #51 from robotdotnet/RemoteChanges
3c3b2c75c Rate-limit duplicate error messages to avoid flooding console.
710bd586d Adds extended Remote Connection Listener
f17d27aac Merge "artf4818: Fix CAN Talon JNI references with underscores."
94629bcb7 Merge "Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now."
4a6f55b61 Fixed the gyro deviation over time test
fdfedd12f artf4818: Fix CAN Talon JNI references with underscores.
ae1171d1b Merge "Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs."
6b356020f Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now.
7041cbc5e Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs.
f24c8b1b8 Fixed robot drive for C++ Simulation
d62256156 Merge "Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405"
61dbd4366 Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405
f913b5de8 Merge "Removed publishing of java sim jar"
bd3e068f3 PDP Classes should support any PDP address
cb4cc6322 Merge "Added -pthread"
838d8abf6 Added -pthread
94bd629b8 Merge pull request #48 from ThadHouse/master
c6ff69079 Merge "Remove maven local as a possible search location"
5d3ac3ea7 Another improvement to HAL-joy getting to ensure it works in future RIO image updates.
f9e87f0cc Removed publishing of java sim jar
73d6e98bf Loads ntcore from path if extracted load fails
880156832 DriverStation::GetJoystickName(): Make work for stick>0.
75a91e24e Remove maven local as a possible search location
6adf4c48c Merge "Fix HALGetJoystickDescriptor()."
026c427a2 Merge "Fixed Simulation C++ API"
63878d8ab Fixed Simulation C++ API
83f902f2f Fix HALGetJoystickDescriptor().
c90a8c586 getTable(): Don't prepend slash if key already starts with it.
f79ed1ab4 Artf4800: Fixes HALGetJoystick*** Segfault
bf89762e8 Merge "fix sim_ds launch script"
bd1e09162 Merge "Added build dir specification for sim javadoc to not overwrite athena javadoc"
2662a7ab0 Initialized the m_sensors variable to fix artf4798.
713d54fd2 Added build dir specification for sim javadoc to not overwrite athena javadoc
75a07fc3e fix sim_ds launch script
6b740e87b Fix C++ PIDController SetToleranceBuffer and OnTarget locking.
ac27f4b64 Merge "Fix onTarget() so that it returns false until there are any values retrieved"
99b6000a6 Fix onTarget() so that it returns false until there are any values retrieved
628811ed0 Correctly set smart dashboard type for AnalogGyro and ADXRS450_Gyro.
c57e749a9 Merge "Fixed sim_ds script library path"
3350926d2 Merge "PIDController feed forward term can now be calculated by the end user"
4dec393c2 Fixed sim_ds script library path
0e9f07d1c Merge "Fixing install script... again"
5765b1397 Use absolute path for NT persistent storage.
2d4304880 Fixing install script... again
e3ce991f1 PIDController feed forward term can now be calculated by the end user
3cd125397 artf2612: Update license in source files.
008fb2e38 Merge "Removed gz_msgs from wpilibcSim"
887f220fe Ultrasonic: replace linked list with std::set.
dd1977815 Removed gz_msgs from wpilibcSim
7a2be548a Merge "Replaced linked list in Notifier with std::list"
5f9300966 Merge "Renamed Gyro to AnalogGyro to match athena API"
d77f5c801 Replaced linked list in Notifier with std::list
6faa51ff4 Renamed Gyro to AnalogGyro to match athena API
9092b74f4 TCPAcceptor: Check for socket creation failure.
b3d28c7e3 Merge pull request #46 from 333fred/master
ed9238546 Merge pull request #44 from ThadHouse/master
1247976a3 Fixes Android Build issues
880bc7db9 Adds a compilerPrefix argument for using a compiler with a different toolchain. Resolves github #45.
ce2245110 Merge "Adds CANTalon to LiveWindow"
dfba97a54 Merge "Fixing the frcsim installer script"
20f23e0e3 TCPConnector: Don't leak socket descriptors.
99b2b6514 Adds CANTalon to LiveWindow
5976baec0 Merge "Fixes CAN devices in C++ library not showing in the livewindow"
932ec8e30 Merge "HAL: Use extern "C" in implementation files."
1c6fe85a7 Fixes CAN devices in C++ library not showing in the livewindow
d9efcbc7a C++ NetworkTable: Add array and raw getters and setters.
e15ca5a41 Added linear digital filters
5e2a07d58 Fix client connection to 2.0 server.
70bc630f1 Fixing the frcsim installer script
6c89f34e4 Merge "Default bufLength for PIDController in Java should be 1"
d542fe429 Merge "Adds WaitResult to Java waitForInterrupt"
351e8599a HAL: Use extern "C" in implementation files.
620836e1c Update .gitignore.
31a39b4e5 Default bufLength for PIDController in Java should be 1
e2ec34090 Keep track of FPGA time rollovers with 64-bit time.
063925e73 Merge "Change C++ Notifier to allow std::function callback."
de4708cbd Merge "Rewrite C++ Notifier to use HAL multi-notifier support."
8bbc1d43b Merge "Rewrite Java Notifier and update Interrupt JNI."
c01146eb0 Merge "Readded styleguide accidentally removed in the reorg"
3e71573c5 Merge "Artf4179: Allow alternate I2C addresses for ADXL345_I2C"
5bc6e1378 Readded styleguide accidentally removed in the reorg
af2f54720 Java: Don't detach thread when releasing globals.
5cee85f92 Fixed some typos in the comments of MotorEncoderFixture.java, a method name in CANMotorEncoderFixture.java, and the README files
951c81f5b Adds WaitResult to Java waitForInterrupt
75d1891a5 Artf4177: Use read byte count for ReadString
376fc6be6 Artf4179: Allow alternate I2C addresses for ADXL345_I2C
91a451f87 Change C++ Notifier to allow std::function callback.
b0de0b738 Rewrite C++ Notifier to use HAL multi-notifier support.
5dc5ed83b Rewrite Java Notifier and update Interrupt JNI.
fe01096e7 Merge "finishing up FRCSim installer"
236ef199a Merge pull request #41 from 333fred/master
acc7fbbf0 finishing up FRCSim installer
1ea5b21dc Added skipArm flag to disable the arm build entirely
e6054f543 ntcore_c.h: Whitespace fixes.
416a238be Merge "Artf4776 Fixes First DIO PWM usage errors"
2aaaed34f Unbreak build on VS2012.
3d2b54a1c Merge "Artf4774 Fixes HAL getHALErrorMessage missing error"
ae99eb676 Merge "Prevent double free in DigitalGlitchFilter"
e2fb1c6d4 Merge "Set correct error message"
4881795a9 Prevent double free in DigitalGlitchFilter
fef8f933d Add SafeThread to fix thread JNI shutdown races.
17b363f3b working on install process for FRCSim 2016
85c3e6a4f Merge "This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places."
2c04cf135 Improved READMEs
dbba4a103 Artf4776 Fixes First DIO PWM usage errors
083c90d37 Added libnipalu to make vision programs link properly
729545809 This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places.
d8de5e4f1 Added ntcore sources zip to the main build
842aba97b Set correct error message
554543c5d Merge "Fixes the sources zip to actually include all sources."
d3f440003 Artf4774 Fixes HAL getHALErrorMessage missing error
20749ed6e Condition java sim build on -PmakeSim flag
44821c3e3 Change how Dispatcher is shut down.
8cc066ecc Fixes the sources zip to actually include all sources.
2540f102b Include sys/select.h on Unix platforms.
684da8d89 Merge "Add SPARK and SD540 motor controllers"
8b3f4aa68 Remove unused member variable.
0537f9d0d Fix mac.gradle.
4b0980fb8 Fixed jar task dependencies. The stripped version of the binary is now added to the jar, not the full debug version of the binary
65e4eeeb7 Fix Windows linkage.
298dc5491 Fix Windows build.
fb486e381 Merge "Added a withoutTests gradle flag for CMAKE feature parity"
32001427d Java: call JNI AttachCurrentThread less frequently.
13496c75b Fixed double free of DriverStation.
2a43813d1 Added a withoutTests gradle flag for CMAKE feature parity
b4c058389 ContainsKey: Compare to nullptr.
88b985be5 Allow building of tests to be disabled in cmake.
d69803804 Repaired simulation build on linux
7528b6b8b Fix NetworkTable::ContainsKey.
0d062fba3 Add Cmake wrappers and unzip desktop ntcore builds
fa903dd9c Add SPARK and SD540 motor controllers
931693345 Merge changes I55ce71c6,I803680c1
31e1041ba Merge "Remove broken and unused HALLibrary.java."
1b03f818a Rewrite CANTalon JNI layer.
cd5765559 Last feature addition for CANTalon java/C++ user-facing API.
4b0407338 Remove broken and unused HALLibrary.java.
0a06fdb52 Clean up FRCNetworkCommunicationsLibrary JNI.
c2ecffe70 Update to latest UsageReporting.h.
ec69c6a86 Fix Solenoid Resource object creation. Fixes artf4758.
bc0c89561 Take 2 on usage reporting.
9e1833058 Report usage for ADXL362 and ADRXS450.
e25d9fc96 Use correct condition variable in DispatchThreadMain().
82d89d3b7 Updated to NI image v18
7ba89306c Merge "Make C++ Analog Potentiometer test work."
c70e710ca Remove raw pointer deprecation warnings.
b1a3ded2f Added desktop classifier publishing for c++ as well
bafe4e56a Notifier: Change back to constructor from ATOMIC_VAR_INIT.
bd2a28f59 Add ADXRS450_Gyro.
6851dee3f Merge "Gyro: Add support for fixed calibration (artf4124)."
de219055f HAL: Add software-based accumulator for SPI devices.
530ce310a Merge "HAL: Add support for multiple Notifiers."
bc6da8eff Add noexcept shim for MSVC.
5d26b1355 NetworkTable: Add override declarations for all functions.
0d7106450 Make TableKeyNotDefinedException::what() const noexcept.
60647a2f8 Turn off Nagle algorithm to decrease latency.
8b0f19a0f Updated Notifier to match changes in gerrit 1171
76ee093e9 Add exception-generating C++ functions, but mark as deprecated.
d0c01ac30 Updates wpilib to use different repos for the different build versions, instead of using classifiers
9e6635ec1 Fixed log compilation on Mac
db91e20ec Updated ntcore build to build both arm and native versions
3cd20d3ff Java: Fix getValue().
6a8fcbc40 Make C++ Analog Potentiometer test work.
5c8214654 comment out halDesktop
f5fe5cfcf JNIWrapper: Fall back to system library if not found in .jar.
10e51f026 Merge "HAL: Increase safety during program termination."
b71cea69c Merge "DigitalInput: Don't crash on Get() of invalid channel."
01b64b5bd Merge "Add support for ADXL362 SPI accelerometer."
e9f98b28c Merge "Reduce overhead of setErrorX() calls."
cd4703065 artf4160 This fixes the java version of the addressOnly() function.
4f535fb77 Merge "Re-enable C++ Pressure Switch Test."
d34ba8866 Re-enable C++ Pressure Switch Test.
b3b03c43c artf4700: Added DigitalGlitchFilter
375a19563 Gyro: Add support for fixed calibration (artf4124).
6d00b77ef Merge "artf4175: Fixes FakeEncoderFixture encoder type error"
93b65aad3 DigitalInput: Don't crash on Get() of invalid channel.
c3c35c604 fix javadoc warnings/errors
03c43a46d HAL: Add support for multiple Notifiers.
07be45af8 HAL: Increase safety during program termination.
c2642f39e Avoid null pointer deref on disconnect.
7182b9a6b Upgrade TCPConnector messages from DEBUG to ERROR / INFO.
ac9e42af3 TCPConnector: Select only IPv4 addresses.
790862db4 Provide default stderr logger for INFO/WARN/ERR levels.
2f8562c23 artf4175: Fixes FakeEncoderFixture encoder type error
bb9988365 Fixed const correctness in casts and unused parameters.
a66eaa434 Reduce overhead of setErrorX() calls.
61760c839 Removed unnecessary ::std::string calls.
055ee0982 Fixed current and potential bugs caught by Coverity
b0fec4089 Merge "Added README, deleted old obsolete building instructions"
906fe65e3 Optimize Solenoid Gets. Fixes artf4730.
84ca2ab0f Merge "Prevent PID tests from hanging."
d998e53f9 Merge "Moved gyro calibration into a separate function so teams can recalibrate at any time after construction"
2f2184e8c Makes HALSetDataSem take a MULTIWAIT_ID rather then a NATIVE_MULTIWAIT_ID
de39877ef artf4153 Adds HAL structure memory management
e3191b0bf Prevent PID tests from hanging.
e162e4d1c Merge "Fix HALGetJoystickAxisType() definition to match header."
5080e5b77 Added README, deleted old obsolete building instructions
f10f8558e Added README for building
9b4dd268b Moved gyro calibration into a separate function so teams can recalibrate at any time after construction
aa95a8c29 Merge "artf4162: Fixes multiple solenoid creations in java"
c935ae2d9 Merge "Adds new CanTalon HAL Functions to C library"
8ad1c07a2 Merge "Remove temporary batch file."
966e4be36 artf4162: Fixes multiple solenoid creations in java
66b28e95a Adds new CanTalon HAL Functions to C library
f36e36424 Fix HALGetJoystickAxisType() definition to match header.
752f5fb4e Merge "Update PCM test to check Solenoid get methods also"
68bb07416 Merge "Arguments to Task are now forwarded to std::thread more correctly"
03ce0a1b9 Remove temporary batch file.
ab90e7aa2 Update PCM test to check Solenoid get methods also
ad64c0b02 Merge "spiSetBitsPerWord doesn't exist; remove from headers."
fec7f3a5e Merge "Make PDP parameters consistent with other HAL functions."
c59e08ee9 Add support for ADXL362 SPI accelerometer.
d59016be3 Arguments to Task are now forwarded to std::thread more correctly
2cd7bc83c Update .gitignore for new tree structure.
964b24361 Make PDP parameters consistent with other HAL functions.
51a35daed spiSetBitsPerWord doesn't exist; remove from headers.
84e793503 std::shared_ptr doesn't need to be initialized with nullptr. The C++ standard already guarantees it will behave as if it is.
26c27756a Updated the definitions file to add overloaded functions, added no-unused-private-field for Mac builds. Gradle also now works with the classifier-based dependency system, rather than having separate repos for every level.
6d854afb0 WPILib Reorganization
c76e60324 Add Java support for raw values.
d98ceb60c Merge pull request #36 from ThadHouse/master
2e050b054 Makes SetTeam use Mdns
c20d34c2b Rename Gyro to AnalogGyro and make Gyro an interface.
e2a455666 Add ByteBuffer interfaces to I2C and SPI.
44ba8823d Merge pull request #34 from JLLeitschuh/feat/functionalTableListener
4c42712b2 Makes ITableListener a lambda function
6a0485a72 Fixed mac lib path
af3facd1c Merge "Notify the Driver Station that code is ready after robotInit is called"
d690974ad Improved error handling in HAL Digital and Solenoid systems.
76756048c Notify the Driver Station that code is ready after robotInit is called
91891081b Merge "Update docs to reflect that PDP can now be at any address."
ff4abd63f Merge "JNI: Don't call HALInitialize() within an assert()."
511c4a4d3 Update docs to reflect that PDP can now be at any address.
967400f18 Merge pull request #32 from ThadHouse/master
a142cc48d Adds StopNotifier and StopRpcServer functions
3469f6733 GetRpcResult: Check m_terminating after wait() returns.
2b6c6f280 Merge pull request #31 from ThadHouse/master
aefeee39a Fixes SetEntryRaw to actually set raw.
27101979f Add extern "C" to ntcore_c implementation.
033520d42 Merge pull request #29 from ThadHouse/master
16e68c348 Fixes parameter type differences for c interface.
94b6073f3 Merge pull request #28 from ThadHouse/master
8fc2eee2b Makes Value::MakeStringArray properly set size
28f55802b Reverted RobotBase debugging changes from Peter.
06372cb14 JNI: Don't call HALInitialize() within an assert().
7023013c4 Simplify JNI interfaces.
927400a43 Added classifiers for networktables to to the pom
2f228380f Updated the ntcore reference and updated the build.gradle file to build with the most recent ntcore commit.
44bd52c44 Update the image version to 15
21124e017 Merge "Revert "Added work around for the missing path on the frc_console_tee command in frcRunRobot""
09962a7c8 Revert "Added work around for the missing path on the frc_console_tee command in frcRunRobot"
969af7b61 Added publishing of an ntcore sources zip
da6611833 Add JNI headers to the exported headers when compiled with JNI
beeefa235 kFramework_Simple was renamed to match the corresponding robot framework's name
26f1bd6a1 Merge "Fixed documentation generation for .inc files"
a34f3f238 Merge "HALUsageReporting::kFramework_Simple was renamed to match the corresponding robot framework's name"
231178c26 HALUsageReporting::kFramework_Simple was renamed to match the corresponding robot framework's name
424efca1b NetworkConnection: bump debug levels a bit.
4d7ea37d5 Fix update message type data.
32a1beb77 WireDecoder: Add commented-out read stream dumping.
d850b2fd7 Gradle build: rebuild on changes to header files in src.
bb52c2441 Merge "Update the library version string for the driver station (artf4708)"
9f5fe63aa Reuse dead connection slots.
03ee425e5 Add additional debug messages.
042e3a323 Update the library version string for the driver station (artf4708)
9200b7c78 Add extern "C" to JNI definitions to catch mismatches.
520d946f6 Fixed documentation generation for .inc files
b793810e4 Fixes for mac builds
9df5f5e27 When running on jenkins, the base name of produced archives becomes the project name. This fixes that to be the correct name.
7fbd0e34d Merge pull request #27 from 333fred/master
77cf3adf6 This commit updates the gradle files to be cleaner. It also builds for the current platform by default, and only builds tests when building for the current platform. Mac builds and VS2015 builds are fixed.
ad3424a69 Fix RobotDrive constructor (4 SpeedControllers).
97a7716a2 Added work around for the missing path on the frc_console_tee command in frcRunRobot
fc0a94af2 Update image version to v14
4da09db20 Revert "Updated the scripts to run as admin and manually start the robot programs. This will be reverted with a new image from NI"
a0cc45a8f Format and style changes. Rebuilt last SRX-related commit with latest checkout.
a3b6535fe Merge "Updated the scripts to run as admin and manually start the robot programs. This will be reverted with a new image from NI"
29299079f Merge "Updated roborio version numbers"
06dea34ff Merge "DO NOT ACCEPT THIS COMMIT UNTIL IMAGE CONTAINS netconsole-host: Don't overwrite system netconsole on deploy"
4972480b2 Updated the scripts to run as admin and manually start the robot programs. This will be reverted with a new image from NI
e73d873df Updated roborio version numbers
4eadd99d2 Merge "Removed simluation eclipse plugin"
1316deaf5 Added LiveWindow support for PID control for CAN speed controllers.
dd0e3e4ab Implement join with timeout (and detach).
d77b3d788 Dispatcher: Remove unused member variable.
64c8f1b48 Reupdated the ntcore version
39c40a9b8 Merge "Revert "Don't overwrite system netconsole on deploy (PENDING NEW IMAGE)""
fbbb8dab7 update the version 12 image number
e44936213 Merge "Revert "Don't overwrite system netconsole on deploy (PENDING NEW IMAGE)""
b4761f8f9 Revert "Don't overwrite system netconsole on deploy (PENDING NEW IMAGE)"
298bc8037 DO NOT ACCEPT THIS COMMIT UNTIL IMAGE CONTAINS netconsole-host: Don't overwrite system netconsole on deploy
bff8eb366 Revert "Don't overwrite system netconsole on deploy (PENDING NEW IMAGE)"
763d81653 Merge "Don't overwrite system netconsole on deploy (PENDING NEW IMAGE)"
4036ddf55 Merge "Fixed miscellaneous typos"
dff6e89b4 Notify local listeners on preferences load.
32af49dbb Fixed miscellaneous typos
c01d057d7 Counter: Explicitly call mode constructor.
490c88a9b Don't overwrite system netconsole on deploy (PENDING NEW IMAGE)
6fc3f1d3d Update netconsole-host binaries (TODO: make part of build)
1309533a4 Updated eclipse plugins to check for the 2016v10 image instead of 2015v23.
57b00d3b3 Fix StorageTest unit test on MSVC 2015.
29e54a989 Adding path back in for OS X toolchains because of Apple lockdown
c384fc97d Fixing up rpath libstdc++ for all versions
9c1ab6987 Merge "Automatically set persistent on new Preferences keys."
d9e9712b0 Merge "Update ntcore to latest version."
e281437c4 Merge "Replaced instances of std::unique_lock with std::lock_guard where possible"
f8c0fb105 Merge "Fix typo in Preferences Test name"
87fc49c66 Automatically set persistent on new Preferences keys.
f93bf82fa Update ntcore to latest version.
c0ecde302 Replaced instances of std::unique_lock with std::lock_guard where possible
c2f5bcc04 Fix typo in Preferences Test name
38e0cd696 Provide compatibility shim for std::make_unique().
898952a2d Merge pull request #26 from Beachbot330/master
202cb8bb1 fix javadoc warnings/errors
f64b05549 Provide DEPRECATED macro as portable version of [[deprecated]].
a480cc65e Removed simluation eclipse plugin
ef3fa53fc Fixed solenoid allocation bug exposed by adding status pointer initialization
049be6870 Always initialize status to 0 when calling HAL from JNI.
c0c91c414 Modified the test scripts to run as admin rather than lvuser until we have the passwd issue resolved
9f859234f Update for latest image, not sure about ctre folder so leaving
89ae8f214 Initialize status in HALInitialize.
4592c90e3 Add protocol specification documentation.
734e9a446 Fix NT_GetEntryInfo implementation signature to match header.
90959defd Provide more extensive listener features.
51064f5e7 Fix client initial flags setting.
3e8afc14b Java NetworkTable: perform string comparion correctly.
3c4b96024 Reverted task dependency changes that do not work
75d9228f3 Fixed networktables build
424bf51a7 Implement local notification.
00f797a32 NetworkTables updates for beta
75358e6c4 This adds a maven publish routine for ntcore to replace the networktables java routine.
e2879b7bf Fix JNI embed location on Windows.
525f88ae1 artf4115: Added MotorSafety to Relays
c21c47f54 artf4000 A fix contributed by James Ward to fix the restriction on the read/write sizes in I2C.
3635299fd Updated Gradle Version
a7feaddd6 Fixed some warnings printed during wpilibC++Sim build
771b5807f Merge "Updated C++ documentation"
c248ce794 Merge "PIDSubsystem.cpp: Use make_shared instead of make_unique."
6db58a2cc PIDSubsystem.cpp: Use make_shared instead of make_unique.
b80d2f665 Merge "Updated gradle version."
4c83259ac C++ NetworkTable: Provide AddSubTableListener().
9c50e9f96 Updated gradle version.
a3effbfb9 Java NetworkTable: Don't use ipAddress when starting server.
90ce262bb C++ NetworkTable: Provide SetPort() method.
23448c827 NetworkTable interface: Provide method to set persistent filename.
d904dfce7 Include the ARM JDK JNI headers.
d9fa086ec Include the ARM JDK JNI headers.
f89c5e150 Use new NetworkTables across WPILib (C++ and Java).
3b91fac19 ntcore.def: Add JNI getEntries function.
f65e69710 Revert changes preventing old user code from compiling.
e42f9b060 Add EntryInfo class (missed in previous commit).
c5d456f3a Add ITable/NetworkTable GetKeys and GetSubTables accessors.
6cbc21942 Add ITable/NetworkTable accessors for new features.
4b06e74a1 Java: Return boolean from put functions.
42f973ebe Java: Don't raise illegal state exceptions unless really necessary.
84e7d5906 Java: Use Global instead of WeakGlobal for callbacks.
51eb96903 Java: Improve robustness against JVM crashes on exit.
6d8e79693 Disable logging on static instance destruction.
f7e603c7d Add DEBUG4 message for messages received during client handshake.
30ad381b6 Notify on entries created/modified during handshake.
5181c4e5b Java: Don't free locals after detaching thread.
a2ec638db Java: Use empty string as default IP rather than null.
ef1e81f93 Merge "Create a WPILib Style guide."
953a2ce80 Implement keep-alives.
e1efb7364 Don't allow update intervals slower than 1 second.
123ba9c67 Use SmallVector instead of malloc buffers for WireEncoder.
d3e63e007 Change cmake to output same jar name (ntcore) as gradle.
21e21d3b8 Fixed the Windows build. Also added ensuring that vs2012 is installed if vs2015 is detecting, and printing an error that actually makes sense if it is not. Finally, added a .gitreview file, so that git-review will be able to autodetect the host and project for ntcore automatically.
702b6de73 Java: Fix crash on JVM exit.
6233b3477 Java: Fix fallback on external library.
07942bf42 Notifier: fix Stop() so it actually notifies the thread.
66f9f73cb Add gradle wrapper.
2dd9eafa4 Enable "make test" on cmake.
b1783cc1d Integrate googletest and googlemock.
c5c615b7d Add sublime text files to .gitignore.
897420a5f Reindent build.gradle.
5d1ab0e82 Merge "Unbreak cmake build."
af01ff300 Updated C++ documentation
550b04cff Merge pull request #21 from ThadHouse/master
46e0ac125 Added documentation to the C interface
22d984a6b Comments the interop functions in the C interface.
8d6f96adb Add NT_FreeCharArray.
1f431754a Simplify C entry point function names.
40093df91 Remove NT_AllocateNTString from ntcore.def.
f3bfee149 Change NT_GetEntry*FromValue to return copies.
9b9b41f40 Reformat to match coding style.  No functional changes.
a451fd3f0 Make string/raw getters/setters use char*+len instead of NT_String.
756520724 StartClient(): Make a local copy of server_name.
e516200e0 Java: Add shutdown method to NetworkTable.
5beaf4577 TCPConnector: Add more info to debug messages.
1ca015776 Java: Add backwards compat shims for array types.
dbe4168d8 Java: allow both object arrays and native arrays.
969916851 Need to return nullptr, not false.
b00b4cb18 Avoid warnings by using sprintf_s on MSVC.
2b9e7c6af Silence MSVC warnings regarding bool conversions.
0d9aaa86c Silence unused variable warning.
28b613d60 Silence MSVC winsock deprecated warnings.
b971c741d Fix JNI for Windows and implement JNI getValue functions.
ecadb117d Merge pull request #5 from ThadHouse/master
4b7dfc025 Implements individual getters and setters for the C interface.
a990859db Initial commit of Java wrappers.
9c576b10d NetworkTable: Don't prefix path with / if key is empty.
b28807d79 Client and server tests: Prefix keys for compatibility.
b488cdd6f Support immediate notify of connection listener.
302cc064c ConnectionListener: Use bool instead of int for connected parameter.
6e4d7ca93 NetworkTable: Protect listeners with mutex.
6c8a5935c NetworkTable: Fix table and key listener behavior.
86445b667 Import llvm::SmallPtrSet.
330ba3993 Unbreak Unix build.
0f4eecebe Fix StorageTest on MSVC.
a3dbe9a80 Fix Windows client.
822dc4583 Close persistent file before attempting to rename.
b8a99690b Fix non-unit tests build on Windows.
48b1120c3 Add some basic non-unit tests.
c27a1cec4 Unbreak StorageTest.
9d4a66a40 Merge pull request #17 from 333fred/master
c846dff52 Fixed Gradle build to actually export proper functions
29f73cb5c Don't dispose in ConvertToC for NT_String and NT_Value.
a8836b766 Value: Use variant of enable_if to fix MakeString/MakeRaw in GCC.
96bf5c24b Merge pull request #4 from calcmogul/master
ab9de550d Replaced time.h with std::chrono
4514e4489 refactored HAL library
9cb9c3beb Logger: Move m_min_level init to header.
2d1bc2f4c Uninline constructors to reduce GetInstance() inlined code size.
c6bed1f46 Dispatcher: Move several fixed initial values to header.
c41341c7e Clean up trailing whitespace.
f0e31487f raw_istream and kin: a few cleanups.
a34143ae7 Update TODO.
ca9ce0f3a Implement automatic persistent saves.
a5ccafd92 Merge pull request #3 from 333fred/gradle-changes
43960b9bc changed windows file separator to unix file separator
29d029fa6 merged from frcsim branch
9458da045 Added building for arm, x86, and x64 with Gradle. This only builds ntcore currently, I'm working getting Google Test to work.
738881099 Unbreak cmake build.
4e4669219 Merge "Fixed wpilibJavaSim build"
8056aaa0a Merge "Accelerometer classes no longer use PIMPL pattern"
80247e98e Use relative include paths in tcpsockets.
293d00543 Turn on logging in unit tests.
a395e3577 Notifier: Fix condition variable race.
8d7cdeabb Implement remote procedure calls.
93b0dab7e Accelerometer classes no longer use PIMPL pattern
099056789 Fixed wpilibJavaSim build
f96b61ef1 artf4106: ISO C++ forbids variable-size array
b5695581c artf4170: Make CANTalon disable on destruction.
2bf3b6bed Replaced WPILib.h includes in integration tests with the minimum required subheaders to improve compilation times
e017f93f1 Fixed examples to build/run with new WPILib versions.
4f8c1dff2 Merge changes I8a9cf402,I8c0f8442
92edf35b4 Make deprecated warnings not error for macs.
e199e3571 Revert "Don't error on strict-aliasing warnings."
e9618df1b Avoid pointer aliasing for double/bits conversions.
c34ba968f Message: Remove ArrayRef-taking ExecuteRpc/RpcResponse.
0cbcfbc3f Import llvm::DenseMap.
cd4ebbd8a Tuned test constants for VelocityPID.
d5922bb03 artf4127: Implemented velocity PID controller
492463411 Don't error on strict-aliasing warnings.
cd53feb19 Log DEBUG() macros: avoid spurious warnings in release builds.
00feb6706 Only use enable_if template on Windows.
b3eed3818 Get build working on Windows (MSVC 2013).
f683a5c63 Export C API functions on Win32.
7d409f071 Add win32 tcpsockets implementation.
4146db6fc Visual Studio 2013 compilation fixes.
a86f65db1 Dispatcher: Implement transmit combining.
53a0531de Storage::ProcessIncoming(): Don't override parameter with local.
7db00575c NetworkConnection: Add a bit more debugging.
5b65bfb64 Storage: Fix comparison for client update-on-assign.
3b0eb56cf Update TODO.
e9073a3cc Implement notifiers.
538a19fd4 Implement GetConnections().
84ff80710 Refactor TCP-specific pieces of Dispatcher.
0dcaf56ed Add NetworkAcceptor and NetworkStream interfaces.
6703968f7 Dispatcher: Fix shutdown order race.
8ec65dbfc Use unique_ptr instead of shared_ptr for m_server_acceptor.
4670ef6ec Refactor StorageTest base class.
35e640017 Storage: Add vector include.
67de7af7b Storage: Use unique_ptr instead of shared_ptr for Entry.
3b207ad2c StorageTest: Fix string comparison for Warn().
b5274a495 Dispatcher: Don't call Storage::GetInstance() internally.
11508b77d Ensure initial synchronization is atomic.
0a1077869 NetworkConnection: Kill write thread when read dies and vice-versa.
ecbf76e94 Update tcpsockets classes to use log framework.
6c0103639 Make LOG() usable from other namespaces.
c2eb4a766 Dispatcher: Provide some INFO-level connection messages.
06e8d835d Provide ostream output for StringRef.
3a71acec5 Automatically reconnect client.
a9af4589a Don't send empty message lists from dispatch.
4356e313e Add logging framework.
afcfdb13a Clean up CMakeLists.txt a bit.
3a570add4 Build as a shared rather than static library.
ead125555 Implement client/server handshaking.
98ad6d1b4 Message: Handle receiving Server Hello.
978cdadda Storage::GetEntryType(): Check for null entry.
cde7782c2 Make StorageEntry a struct and move into Storage class.
138ebf5b4 Storage: generate messages rather than intermediate updates.
f0e3bb516 artf4165: D term of PID controller now uses change in input instead of change in error
918cde0c0 artf4155: Move Port.h from hal/lib/Athena to hal/include/HAL
43f16510f moved ctre includes from C++Devices into Hal
959f8775d Merge "Temporarily disable MutexTests until artf4167 is resolved."
0d70a39f6 Merge "Replaced ::std with std for readability/consistency."
ba57c7f24 Make gradle actually build command tests in C++.
a127bca0e Push down null check for m_queue_outgoing.
83be99e78 Immediately process incoming messages.
5b5e3ae6a Dispatcher: Start implementing processing of incoming messages.
f6576b18f Storage: Add more Dispatcher accessors.
9a621e927 Various NetworkConnection improvements.
4b575e3e7 Replaced ::std with std for readability/consistency.
c01f2977a StorageEntry: Make value() const.
683c53bab Message::str(): Return StringRef.
5823e3c78 Storage: Add CreateEntry function for Dispatcher use.
cbc372c71 Temporarily disable MutexTests until artf4167 is resolved.
a0d30ffef Storage: Use StringMap instead of ConcurrentQueue for updates.
fb1b82e23 StorageEntry: Also store copy of name here.
3bc5699ba Storage: Provide accessor for global mutex.
18659257d Storage: Make setters globally atomic.
3c7cb363b NetworkConnection: Prefer emplace to push.
787d39851 Update gradle build to match cmake build.
ae070aca7 Value: Add type check functions.
63dd895e6 Pass CMAKE_TOOLCHAIN_FILE down to external project (gmock).
507b083e7 WireEncoderTest.cpp: Don't test compiler truncation.
13bc05d9e leb128Test: Fix incorrectly sized constants.
3f24b8687 ITable: Add Persist function to make a key persistent.
1368f0ec8 NetworkTable: Use networktables.ini as persistence filename.
196fcf791 ITable: Const'ify getter functions.
6bccf528d ITableListener.h: Forward declare ITable.
1fc03803c ITableListener::ValueChanged: Don't make key const.
f74ca87e2 Merge "NamedSendable::GetName() is now const qualified"
5756be5c2 Merge "Fix std::unique_ptrs using incomplete types"
2da3f7f89 Merge "Replaced all uses of DISALLOW_COPY_AND_ASSIGN macro"
cf965d26a Merge "Make test scripts spawn driverstation as admin."
af3eaae09 Merge "Comment out SetPriority() in Task constructor."
8fa0e6c91 Add shims for old NetworkTable interface.
6f940bcaa Add immediate_notify and is_new to entry listener.
79f732f23 Import llvm::SmallString and dependencies.
593bc2844 Move StringMap.h and StringExtras.h to public includes.
9c204533e Value: Disambiguate std::string&& and StringRef.
4c14f7823 ntcore_cpp.h: Comment cleanup.
4aa2d65bb Storage: Use std::forward and emplace for updates.
bb5848a03 ConcurrentQueue: Add emplace function.
9f728b850 Add MSVC specific compiler flags.
9c2678fdb Make test scripts spawn driverstation as admin.
3bbeccfbb Replaced all uses of DISALLOW_COPY_AND_ASSIGN macro
b4c65dc21 Make global instance init thread-safe.
5df62ac17 Storage: Disable use of update queue by default.
0979c1c9c Storage, NetworkConnection: Don't in-place construct atomics.
8bbe5f9fd ArrayRef: Add portability checks for constexpr.
881dcd08e Fix std::unique_ptrs using incomplete types
3cd3d1691 Message: Use #define instead of constexpr.
6b2fb02be Dispatcher: Don't in-place construct atomics.
8938a1981 Dispatcher: Avoid warning by using for(;;) instead of while(true).
158ae6181 Dispatcher::SetUpdateRate(): Explicitly convert to int.
6bd2534f0 Comment out SetPriority() in Task constructor.
451c4e81c NamedSendable::GetName() is now const qualified
eb7d55fd5 Fixed classes with undefined constructor.
c776324f9 Merge "Make the teststand run as lvuser instead of admin."
98f2d0810 Changed const char* -> string in most of wpilibc.
534ea134a artf4154: Get rid of raw pointers in C++.
fd4c16965 artf4149: Removed references to VxWorks
571256506 Removed dependencies on pthread from frcsim.
3f59f3472 artf4156: Replaced synchronization primitives with C++11 equivalents
98d45777c Remove unnecessary llvm qualifier.
d05902207 Add logging to TODO.
c9260ea78 Change GetEntryTypeFunc to std::function.
c9ca2f902 Add TODO list.
29691e0ac StorageTest: Add LoadPersistent and SavePersistent coverage.
a3fcce891 LoadPersistent: Fix various bugs.
8db016c22 SavePersistent: Sort in name order, fix list commas.
b0802f3e2 StorageTest: Add fixture classes, add GetEntryInfo test.
e4731a4e4 Storage::GetEntryInfo: Actually filter on types.
67ae9e1ba Add initial set of unit tests for Storage.
d6afbc56c Storage: Delete functions now delete from map.
ffb54872c Storage::SetEntryFlags(): Fix deadlock, check for empty name.
ead6b4960 Storage: Use make_shared instead of reset.
21b7acc39 Storage: Make testable, make EntriesMap typedef private.
173111c64 ConcurrentQueue: Make mutex mutable.
cf18355fe Unit tests: Prefer constructor to SetUp() in fixtures.
1cc148848 StartServer: Take persist_filename as a StringRef.
ec5490434 Tighten up C++ API implementation a bit.
0a18d2e57 Storage: Make individual entries thread safe.
c08e2ed8f ConcurrentQueue: Add size() function.
2437f06c7 Implement majority of Storage functionality.
9b7e26576 Continue implementing client.
c4a7f6ec9 SavePersistent: Safely save through temp file.
d05656b71 StorageEntry: Make data public, remove accessors.
04789d9ae SequenceNumber: Add default constructor.
77acf1f35 Implement Flush() API.
412e8034d Start implementing client and server.
a6162ba99 NetworkConnection: Trigger threads as necessary in Stop().
7c5117860 TCPAcceptor: Add shutdown() function.
899c48912 TCPStream: Shutdown on close.
3062e1e74 Message: Add data accessors.
854bfba7c Value: Set last_change field.
1d4de091f Implement nt::Now().
b8f044f8e Make the teststand run as lvuser instead of admin.
440916cf2 Fix string terminator in char** output.
3f4feb2f5 Remove unused variable.
3025a7e51 Finish C wrappers.
fcbd2751b Provide C++ API.  Move all classes to "nt" namespace.
56f1481c2 Add Dispatcher.
2ea20b8e8 Value: Use NT_Value for internal storage.
9906116d2 Don't base Value on NT_Value.
555725a05 NetworkConnection: Don't explicitly declare default move constructor.
13cbf4e28 Message: use shared_ptr.
beb92e6cb NetworkConnection: Ignore duplicate Start() calls.
4d3fb3c49 Storage: Use unique_ptr for instance.
dbd0c9830 Message: implement MsgType constructor.
53fb70251 ConcurrentQueue: Add empty() function.
e64070824 Start implementing network connectivity.
8fdaf61ef Use typedef for GetEntryFunc function pointer.
a5713252d Message::Read(): restructure to read directly into Message.
f5ec5e180 Message: don't explicitly convert StringValue.
27e0f22c0 StringValue: Implictly convert/assign from StringRef.
2016bcb37 Replace MessageReader and MessageWriter with Message.
1760d3201 MessageReader: Rename Run() to ReadMessage().
9808c6e62 Remove extra whitespace.
071b278b7 MessageReader: Use rvalue refs and std::move for handlers.
f4673f312 WireDecoder::Read(): Use const char instead of plain char.
8ee4c36e0 MessageReader/MessageWriter: bring in proto_rev().
0f876d198 Use StringRef instead of NT_String in various places.
eecf0deee Add WireDecoder test and finish WireEncoder test.
6aa32e875 WireDecoder: Add overloads for StringValue and Value.
a55b6565b WireDecoder ReadDouble: Use ref cast.
c1c0c8d41 Optimize Write16/32/Double a bit.
163477352 Finish WireEncoder tests and fix a bug.
547e2ad72 leb128 test: naming consistency
ca9064811 Add copyrights and header guards.
958fbaa81 ValueTest / StringValueTest: Make conversion function static.
fa19a54ab Separate ValueTest and StringValueTest into headers.
c9c1b7e5d Rename test files.
4d5e5c82d Start WireEncoder testing.
9bd4a5ecc Add proto_rev accessors to WireEncoder and WireDecoder.
09d6619a5 Add LEB128 test.
e7ba40dcf ReadUleb128: Take const buf.
ae42eee8e More Base64 fixes (correct decode return value).
1e9b9b9a3 Base64: Fix a little differently.
1225d3ef7 Slight whitespace fix.
407ded6b8 Use ASSERT_EQ with correct order of arguments.
c7b5266f9 Add Base64 tests and fix a couple of bugs found.
5a0fccc9c Add unit tests for StringValue and Value.
b66fb68f2 Finish LoadPersistent().
cb30bb5d7 Rename base64 to Base64.
40ebb9a84 C++-ify base64 functions.
b7a87bb6f Make StringValue noncopyable.
e38cce46a Add missing llvm headers.
f6deafee2 Add type-safe wrapper around NT_Value and NT_String.
8d6a0786c Move all C api functions to ntcore.cpp.
9ee307066 Storage: Make entries private, add accessors.
05e084364 Start implementing LoadPersistent.
69e91244f Rename make_StringRef to MakeStringRef.
e37a9d81f Cleanups and commenting.
95d0736bb More coding guidelines name changes, and use "using".
de4ba1aa3 Fix up naming of Base64 and Leb128 functions.
d7ca3343b File renames.
cee77a322 Reformat per new coding guidelines.
f5a82be9e timestamp.cpp: A few cleanups.
2a1ccaff1 Add timestamp function NT_Now().
b195dfbe0 Add ConcurrentQueue implementation.
0aab4c27e Use nullptr.
42ad98567 Use C++11 deleted functions for noncopyable.
c06e62541 Fix code style for pointer and references.
1a48e75d4 Implement SavePersistent.
a896a3ade Import LLVM StringMap et al.
62ef9e1c6 Remove extra Reserve calls.
047cccda8 Refactor WireEncoder.
dbed3fea6 Move remaining read functions.
b88a9295b More refactoring.
c4d4679ed Finish EXECUTE_RPC and RPC_RESPONSE handling.
c2063f3a8 Refactor MessageReader into WireDecoder.
03bae15b9 ntcore: Implement RPC_RESPONSE reading.
842fb957a ntcore: Initial commit.
7f5ee01d3 Merge "DISALLOW_COPY_AND_ASSIGN macro now uses the delete keyword and allows move assignment and construction"
f4d84cdd4 Fixed potentially uninitialized value warnings emitted by clang
974e301a8 Merge changes I3d35139f,I05bb06f6,I4a3db0f1,I452b4794,If281e46a
9dc7bb4ec Merge "Changed 'mvn.bat' to 'mvn.cmd' such that the eclipse plugins can build on Windows with the current version of maven."
1ab3ea670 artf4107: Removed most "Init" functions from classes
e4a8aacc5 artf4107: Replaced throw() with noexcept
faedfa6ed artf4107: GetInstance() calls are now atomic
368ad30d3 artf4107: Uniform initialization syntax introduced
b1befed14 artf4107: clang-modernize was run on WPILib
045a9b32d DISALLOW_COPY_AND_ASSIGN macro now uses the delete keyword and allows move assignment and construction
7a711a21f Make jenkins fail if tests fail catastrophically.
1c3882ec7 Create a WPILib Style guide.
35ccbb9c9 Fixed bad initialization of PDP.
8794f7d63 Changed 'mvn.bat' to 'mvn.cmd' such that the eclipse plugins can build on Windows with the current version of maven.
2c392d181 Fixed comment problem in GyroMecanum cpp and made other small fixes to sample programs.
7dcbe171b Merge "Major formatting changes (breaks diffs). No code changes."
6fefcc779 Merge "Added new sample programs to the eclipse plugins for C++ and Java. Modified examples.xml for the corresponding added programs."
7eb8550bd Major formatting changes (breaks diffs). No code changes.
bd64d9a7e Added and implemented GetInverted method.
0122086d2 Added tests for motor inversions.
ae90e5f6d Added new sample programs to the eclipse plugins for C++ and Java. Modified examples.xml for the corresponding added programs.
2d71c1c1c Merge "[artf4062] Eclipse now throws error on bad build."
0ad0b567e Merge "Artifact artf3896, commented out line that deletes the folders/files in wpilib/tools so teams smartdashboard extensions don't get deleted on plugin update."
401340213 Add PIDInterface for PIDController and CAN devices.
27e450967 Add CANSpeedController interface in Java.
6ce98056d Merge "Added Ultrasonic example program in Java and C++, updated examples XML."
c7944d76a Merge "Add check for CPP Perspective to CPP Launch Shortcuts. Fixes artf2627"
9c0df8c8f Add check for CPP Perspective to CPP Launch Shortcuts. Fixes artf2627
5598445a6 Improve const correctness.
c357d1dca Added Ultrasonic example program in Java and C++, updated examples XML.
c6b790cad Artifact artf3586: Added include for stdint.h in WPIErrors.h file, so the header can stand alone.
ead60eaab artf3622: Each Solenoid creates handles for all other Solenoids as well
39130721d Artifact artf3528, Added support for PDP's having addresses other than zero, and added ability to have multiple PDP's.
8d17270dc Merge "Added functions to get names, HID types and isXbox descriptors from joysticks."
bf4ccf13d Added functions to get names, HID types and isXbox descriptors from joysticks.
ee824680d Re-adjusted some PID values for the tests.
d252ed528 [artf4062] Eclipse now throws error on bad build.
3de0f162b Merge "Added modified Google Style Guide."
13948e819 Merge "[artf3943] Add LiveWindow mode to CANTalon."
ac80114c4 Merge "Fixed BuiltInAccelerometer Tests, changed expected axis of gravity from the z-axis to the y-axis."
7100052c5 Added modified Google Style Guide.
4b45b6a19 Artifact artf3896, commented out line that deletes the folders/files in wpilib/tools so teams smartdashboard extensions don't get deleted on plugin update.
b5ce28135 [artf3943] Add LiveWindow mode to CANTalon.
e48edbebd Fixed BuiltInAccelerometer Tests, changed expected axis of gravity from the z-axis to the y-axis.
0862210f5 Fix bad suffix for staticLibArchiver on macs. Needs to be tested on mac.
1c9dffc30 Commented OS X workaround in build.gradle
32251fe4a Merge "Makes the build work on Mac (needs to be tested on other platforms)"
08487b56a Merge "Changed calculate method to protected and virtual"
40c6fbe24 Makes the build work on Mac (needs to be tested on other platforms)
33f012a60 Merge "CANTalon: Add getter methods for soft limits."
fafc8635c Merge "add some comments etc documenting various not-implemented things"
8674ac20f Merge "Added missing archiver prefix for wpilibc"
2296d6560 Changed calculate method to protected and virtual
bbffc825e Merge "Change wpilibcZip to use "lib" instead of "libs" to match where eclipse plugins expect libraries"
682bc171f Change wpilibcZip to use "lib" instead of "libs" to match where eclipse plugins expect libraries
47088c035 CANTalon: Add getter methods for soft limits.
4cac5a957 add some comments etc documenting various not-implemented things
675fbc032 Test bench fixes and updates. Attempting to make it possible to have a stable build again. Gyro test still seems a bit unreliable.
880eae142 don't use uninitialized variables
d1d28e70e Merge "This commit adds JNI bindings for the C++ Notifier."
269bf9db6 Merge "Removed confusing error on deploy."
671954a66 Merge "Add MSYS Makefile option for cmake to Building.md as well as a note about the warning that's generated. Fixes artf3748"
19526361f Merge "Add DisabledInit to C++ Command Based template. Move and modify comment on Java version. Fixes artf3840"
a9593854d Merge "Fix usage reporting for new motor controllers and Analog output"
9cda6e3ff Merge "Tuned some values in tests to account for hardware imperfections."
0853dc8e5 Merge "Fixed access issues and 'overridableParamatar' misspelling."
0b7bb41b2 This commit adds JNI bindings for the C++ Notifier.
90f05dd31 Removed confusing error on deploy.
9b6e48e14 Add MSYS Makefile option for cmake to Building.md as well as a note about the warning that's generated. Fixes artf3748
5a094879b Add DisabledInit to C++ Command Based template. Move and modify comment on Java version. Fixes artf3840
8b71ddc30 Fix usage reporting for new motor controllers and Analog output
1805968d7 Tuned some values in tests to account for hardware imperfections.
01d73d04f Added missing archiver prefix for wpilibc
1e4e0bacd Gradle Build
569f02de8 Fixed access issues and 'overridableParamatar' misspelling.
0dffbd863 Catch exception when instantiating USB camera if camera isn't present
0e46592ad Don't send DS errors if no DS.
d2edb80da Merge "Use PCM Device ID when getting status. Fixes artf4014."
6159fde98 Set POV values on error to return -1 rather than 0 AND bump the version number (artf4005)
7bdd91a05 Use PCM Device ID when getting status. Fixes artf4014.
605bb45e0 Fix endian order of buffers in AnalogTrigger.java
215002e48 Fix analog trigger bug with MXP mapping (artf4010) Add the interrupt code for MXP mapping and analog triggers Took out the unneeded static definition Change-Id: I9a3483ee8f806b46b4349845e7a189f497c36916
964475c72 Bump the version number to 1.1 for the network table changes
bfa4bbaf7 Final move of Dustins network tables patches
39158754d Partial merge of patch from James Killian Narrows scope of locks in write manager added delay to incomming stream monitor loop
048b02e6c Applied patch from Dustin Spicuzza to fix robot hang
130319b77 Merge "Fixes artf3989: Java SerialPort no longer ignores return value of serialRead"
0b414b1bd Merge "Disable Encoder console spam introduced in e13720b."
8a244cc9d Merge "Make index encoder constructors work."
397162390 Merge "Correct energy to be in Joules rather then millijoules. Fixes artf3940"
bbc216f99 Disable Encoder console spam introduced in e13720b.
72a56e6a8 Make index encoder constructors work.
102c992e5 Fixes artf3989: Java SerialPort no longer ignores return value of serialRead
9f057fddf Correct energy to be in Joules rather then millijoules. Fixes artf3940
e614217d4 New netconsole-host that overwrites the old one on every deploy
9b7042a51 Fix errors in Vision examples.
081d7f736 Merge "Applied updated DIOJNI file from Ron Rossbach. Fixes artf3982."
352c021b1 Merge "Updated the Java CameraServer to use the new USBCamera and prevent large camera data copies. Fixed a small bug in USBCamera to prevent reinitializing a camera if it is already initialized. Also fixed some issues with the getJpegSize function to correct for Java incorrectly casting 0xff to an integer, and the byte buffer limit being set incorrectly."
5f90bf167 Merge "Enable nivision Java wrappers to be generated with Python 3."
445eafefb Merge "More Java nivision wrapper fixes."
a917f63e7 Merge "2015 Color and Retroreflective vision samples for C++ and Java"
2228361f3 Merge "Add SetCANJaguarSyncGroup to RobotDrive"
aaa599fa2 Updated the Java CameraServer to use the new USBCamera and prevent large camera data copies. Fixed a small bug in USBCamera to prevent reinitializing a camera if it is already initialized. Also fixed some issues with the getJpegSize function to correct for Java incorrectly casting 0xff to an integer, and the byte buffer limit being set incorrectly.
5d95bbb33 Enable nivision Java wrappers to be generated with Python 3.
d0258923e More Java nivision wrapper fixes.
91ccc5d10 2015 Color and Retroreflective vision samples for C++ and Java
9fda92005 Merge "Add @file annotation so files are picked up in doxygen"
e9dd72ac7 Merge changes I0ec40c14,I4d099e62
cce1bbc1f Merge "Update to point to Gazebo3.2 header location."
9fc79c41d nivision Java wrappers: implement our own error text function.
1dba91688 Add @file annotation so files are picked up in doxygen
ce7a56284 Fix a few major nivision Java wrapper issues.
34d515deb Make sure Utility file is included in doxygen output.
cff103d15 Merge "Add encoder indexing in Java"
1b5cabc0d Merge "Add encoder indexing support in C++"
9fcfc5b3b Merge "Added the C++ implementation of Peter Johnson's UsbCamera. Rewrote CameraServer to use the new USBCamera implementation and get rid of some unnecessary copying of the entire image."
f181c7013 Merge "Add USBCamera class."
46c40da53 Merge "NIVision: Add dx functions."
1c05a982e Merge "Synchronize CANTalon documentation between languages."
ba7f56833 Add SetCANJaguarSyncGroup to RobotDrive
7600473da Applied updated DIOJNI file from Ron Rossbach. Fixes artf3982.
9f04b7958 Fix Timer.hasPeriodPassed().
2282a11a7 Added the C++ implementation of Peter Johnson's UsbCamera. Rewrote CameraServer to use the new USBCamera implementation and get rid of some unnecessary copying of the entire image.
1a2344bdd Add USBCamera class.
ae9a7d19f Merge "Miscellaneous AnalogTrigger fixes"
e1141b9f2 Merge "Disable C optimize setting so better C++ doc is generated"
ff71aca57 Merge "Update docs to add missing channel numbers"
837671e95 Merge "Document that PDP must be address 0 and add/fix measurement units"
38c0308fc Merge "Correct path of sfx so that it can be launched from eclipse menu"
f21e4a9da Miscellaneous AnalogTrigger fixes
e13720bb9 Add encoder indexing in Java
0cf7d6f45 Update docs to add missing channel numbers
a2dfffedd Add encoder indexing support in C++
87e1df068 Fixed a null pointer exception in the mecanum drive sample program (Fixes artf3974)
f19c290fd NIVision: Add dx functions.
5c342a8e1 Document that PDP must be address 0 and add/fix measurement units
f7117aac4 Correct path of sfx so that it can be launched from eclipse menu
05cd0627b Update to point to Gazebo3.2 header location.
2bc83bd80 Disable C optimize setting so better C++ doc is generated
79fbbbc0d Synchronize CANTalon documentation between languages.
359a15591 Merge "Make 3 axis Accelerometers work in LiveWindow. Fixes artf3902."
fb7f5e9de Add methods to retreive the FPGA index for counters and encoders and return the encoding type as an integer Change-Id: Iaa4062ec124b968b27e7c812cc754032fcb354d2
7db63055f Make 3 axis Accelerometers work in LiveWindow. Fixes artf3902.
b94341a23 Merge "Update nivision JNI generator to correctly gen Priv_ReadJPEGString_C."
afbd70d39 Update nivision JNI generator to correctly gen Priv_ReadJPEGString_C.
ceccd9508 Add PDP to LiveWindow for C++
91b981248 Merge "Add PDP LiveWindow code"
0dbc40e51 Added missing .java file
5ce89b338 Added a Java AxisCamera sample program
8ccf3d9c1 Add AxisCamera Java class
230fa2d16 Add PDP LiveWindow code
d165f57e6 Clean up misc documentation
2d15b6b03 Merge "Update Java install URL to the new URL with the java installer."
5d9baa44e Merge changes Ie40db6c9,I0b5e752e
822416d2f Merge "Write lib version from C++ (fixes artf3788)"
3fe726d85 Merge "Update docs for C++ (fixes artf3761 and artf3953)"
4c0b2c18a Write lib version from Java (fixes artf3788)
b4c5a3af7 Update Javadocs for Java (fixes artf3761 and artf3953)
eddb0b20b Write lib version from C++ (fixes artf3788)
6d8e782f5 Update docs for C++ (fixes artf3761 and artf3953)
036832238 Update Camera templates to make C++ and Java more similar.
19e05ff52 Update Java install URL to the new URL with the java installer.
a6aef54ef Merge "Axis camera C++ example program"
32f3bea46 Axis camera C++ example program
9aacf5eb2 Merge "Artifact artf3925 : PCM : Can't find any user facing java/C++ API for getting/clearing PCM faults"
f3484686c Fix some typos in AxixCamera.cpp
b78b14bf5 Merge "Add AxisCamera C++ class"
c08dc9865 Merge "Add the 2014 C++ vision classes back in"
9fbffd88c Merge "Change the java intermediate vision filename to match c++"
6c15a3600 Change the java intermediate vision filename to match c++
548941dd9 Artifact artf3925 : PCM : Can't find any user facing java/C++ API for getting/clearing PCM faults
9d8418c14 Add AxisCamera C++ class
326bf4fc9 Merge "Updates the InterrupatableSensorBase documentation to reflect the bug described in artf3623."
3c4a1d9a1 NIVision Forward Port
768702826 Updates the InterrupatableSensorBase documentation to reflect the bug described in artf3623.
a55f34646 Encoder/Counter Fixes
53473e21b Add the 2014 C++ vision classes back in
b4097fbd5 Merge "ClearStickyFaults() should not take a param, just clears sticky flags in a one shot manner. GetCompressorFault() => GetCompressorCurrentTooHighFault() 	There are now three compressor related faults so getters needed to be more verbose. New getters()... 	GetCompressorCurrentTooHighStickyFault 	GetCompressorShortedStickyFault 	GetCompressorShortedFault 	GetCompressorNotConnectedStickyFault 	GetCompressorNotConnectedFault"
3b7211455 Merge "Fix imports on vision examples so they compile. Fix Typo."
cc36454b9 Merge "Add C interface for CanTalonSRX::SetModeSelect(modeSelect, demand)."
aae20ddbf Artifact artf3945 : CANTalon : Motor Safety Object never feed
ffd906176 Fix imports on vision examples so they compile. Fix Typo.
9ffdea188 Add C interface for CanTalonSRX::SetModeSelect(modeSelect, demand).
46dc99a11 Adding SFX to tools zip
91d714d2e Merge "Single line bug in CanTalonSRX::GetAnalogInVel().  return value was not being sign-extended."
9a28aaaa7 Merge "Change expected voltage on CAN tests"
96a76ba89 Single line bug in CanTalonSRX::GetAnalogInVel().  return value was not being sign-extended.
d26059a4f Change expected voltage on CAN tests
ee0d83530 Remove an extra constructor from a bad merge
6080a3b18 Duplicate of gerrit 739; changed method from public to private.
3d06a763a CanTalon : Adding config routines for limit switch normally open vs normally closed. They already existed in Labview, so this will keep parity New C++/Java funcs ConfigFwdLimitSwitchNormallyOpen ConfigRevLimitSwitchNormallyOpen
e1480ec79 Noticed that if changeControlMode() is called every teleop loop, it causes motor to stutter as it enters and immediately leaves kDisabled. A simple improvement was to only perform the disable-before-next-set strategy if the caller's request mode is not equal to the current mode.
a5d9ba412 Second pass through all the HAL functions and cpp/java API. Filled in some parity holes between java and cpp.
2434515d4 artf3913 Increased wait delay to 4ms to cover worst case delays for solicted signal getters. Specifically.... -Get firmware vers -Get P,I,D,F gains -Get IZone, Get CloseLoopRampRate -Get IAccum (integral accumulator)
741618250 Merge "C++ CANTalon was missing SetIzone.  Added SetIzone to match java, and made the set/get Izone integral."
8b8d7e77c C++ CANTalon was missing SetIzone.  Added SetIzone to match java, and made the set/get Izone integral.
c093a553e changed variable type of closeLoopRampRate in setpid to double from int.  It's represents V per sec since it calls setCloseLoopRampRate() underneath.
a1375e58c Merge "Don't fail silently if DIO or PWM allocation fails"
15ff7f503 Merge "Added the getButtons method back that reads all the buttons at the same time"
c17ba98f7 Added the getButtons method back that reads all the buttons at the same time
dee755ab1 Merge "renamed param to match function name."
92c54f5f5 Merge "Image v23"
1fde00643 Merge "Fixed post4066 bug: Prestart() inaccessible."
47443b4e1 Fixed post4066 bug: Prestart() inaccessible.
f01e5b557 fixed robotCommand in src
198e2eed1 ClearStickyFaults() should not take a param, just clears sticky flags in a one shot manner. GetCompressorFault() => GetCompressorCurrentTooHighFault() 	There are now three compressor related faults so getters needed to be more verbose. New getters()... 	GetCompressorCurrentTooHighStickyFault 	GetCompressorShortedStickyFault 	GetCompressorShortedFault 	GetCompressorNotConnectedStickyFault 	GetCompressorNotConnectedFault
22c420755 Image v23
bea9eb0ef renamed param to match function name.
b72eb4b81 Merge "added Java vision example programs"
0d8c45472 Don't fail silently if DIO or PWM allocation fails
d61d491a0 added Java vision example programs
786e844a9 Merge "Fix ControllerPower 5v faults and add javadocs. Fixes artf3918."
170b5860e Fix javadoc compile errors that broke build.
26c50ebe0 fixed java CameraServer
46c659d6b Enable Java 8 doclint checking (except for missing tags).
6fdd49108 Fix javadoc warnings.
fe4535dfa Merge "Resolved artf3579: robot can no longer be enabled until robotInit() finishes in IterativeRobot; similar options available by overriding prestart() for other base classes."
636e2e13a Wrong package declaration in camera server
d3f5b7466 added CameraServer.java
8116bbd15 Merge "Fix various bugs in nivision wrappers."
37052246a Fix various bugs in nivision wrappers.
a649d3b55 Fix ControllerPower 5v faults and add javadocs. Fixes artf3918.
6a7e7cf61 Two param constructor added to C++/java CANTalon so caller can optionally specify the periodMs at which the talon control frame is sent. The param is capped in the HAL C++ class to [1ms, 95ms] so that zero and negative periods are caped to 1ms, and so that caller can't pass an absurdly large value, which causes TALON is appear disabled.
77997e52f Merge "added PDP methods to javalib"
e655072ef Synchronize access to buffered Joystick data.
0427fc34c Merge "Check for negative button value and add missing newline."
e33d80be1 Merge "Add USB serial port option. Uses kUSB for ALSR3"
8381eee18 Merge "Wrap IMAQdx functions."
1c24096cc Resolved artf3579: robot can no longer be enabled until robotInit() finishes in IterativeRobot; similar options available by overriding prestart() for other base classes.
3a684d28b PWM timing change for SP and SRX. added 3us to outer bounds
8786b242b Add USB serial port option. Uses kUSB for ALSR3
b29a4bebf Check for negative button value and add missing newline.
db0b42101 Wrap IMAQdx functions.
8efe99827 Make VictorSP and TalonSRX use correct bounds (give an extra 1us on min and max to ensure saturation). Fixes artf3914 for C++ and Java
ac6019884 Merge "Change vision defaults from "cam1" to cam0". Add some error reporting to Intermediate Vision example"
8a5ee71fd Merge "Make SetImaqError actually set the error"
af4ce1074 Merge "Image v22"
763604139 Don't use raw type for HALSetNewDataSem
745489fec Image v22
04f9ca4fe Change vision defaults from "cam1" to cam0". Add some error reporting to Intermediate Vision example
ca5dfbe49 Make SetImaqError actually set the error
07619a37a Merge "Add C function wrappers for CanTalonSRX."
34d3d756e Merge "Update javadoc for RobotDrive. Mecanum methods are implemented."
61a5fcce1 Merge "Squashed commit of the following:"
82c4563d3 Merge "getHALErrorMessage(): Add missing CTRE errors."
fa337bc74 Add C function wrappers for CanTalonSRX.
8ae7e973f getHALErrorMessage(): Add missing CTRE errors.
574f2e692 Java nivision: Add RawData to wrap void*.
827341caa Squashed commit of the following:
dd272e6bc Merge "Add Java nivision wrappers."
3bdaa63a2 Update javadoc for RobotDrive. Mecanum methods are implemented.
41b393c21 Merge "Fixed minor issues in CANTalon. Fixes artf3884, 3885, 3887."
11cf860ec Merge "Check and coerce rumble inputs to range of 0 - 1."
2168d2cb7 Merge "Require Jaguar version v108 or higher."
430722c4a Add Java nivision wrappers.
497f38fe0 Check and coerce rumble inputs to range of 0 - 1.
9f2dcdeab Fixed minor issues in CANTalon. Fixes artf3884, 3885, 3887.
ac07142e4 added PDP methods to javalib
19a7243bf C++ Added Get/clear routine for IntegralAccumulator Added missing status check in GetFirmwareVersion().  I don't expect this to affect anything.
e3ac0b628 Merge "Fixed issue in setting CANTalon values."
709a88ad6 Fixed issue in setting CANTalon values.
6b844b52e comment change and added SetModeSelect(int modeSelect,int demand) {
9056edf93 commented out system.out.prints in CANTalon.set()
ff2ea1287 Merge "Added C++ versions of the joystick query functions"
b41690b38 Added C++ versions of the joystick query functions
ce8e65d41 Merge "Changed AnalogPotentiometer to use angle specification and rail voltage."
66622b43e Fixed accidental confusion between seconds/milliseconds.
568b842c7 added setPosition to java. Great for zero-ing your relative sensors, also exists in cpp and LV.
4d142cdaf commented out throws in getP,getI,getD,getIzone, getRampRate. Getting/setting these should be available all the time.
2ae8f40a5 Changed AnalogPotentiometer to use angle specification and rail voltage.
483331657 Added more Talon SRX documentation and PID samples.
16f9db30a Merge "fixed bug  artf3676 : Typing in a project name into the create command dialog in eclipse is broken"
e092742f4 fixed bug  artf3676 : Typing in a project name into the create command dialog in eclipse is broken
4f6fa2482 added joystick and driverstation counts for POV, buttons, and axis
52408e265 Add classes for VictorSP and TalonSRX PWM control.
d986ffac8 Merge "Java-Installer"
bc3c5447e Java-Installer
b125e6b40 Merge "Various getters and setters added to C++. usleep added to the getters that require a little time for solicted response (getPIDF, getIzone, and getFirmwareVers. Tested against the TALON SRX unit test originally written for CanTalonSrx HAL class."
88a043bda Merge "Change Periodic Status rate to 20ms. Jaguar firmware v109 fixes issue with periodic status sending."
c57d01af9 Merge "Add USB IP to deploy fallbacks and make fallbacks work."
6abde412c Merge "Image v20"
5e6cd0bf9 Merge "Implement Joystick Outputs and Rumble (fixes artf3807)"
ec03c3068 Add USB IP to deploy fallbacks and make fallbacks work.
45f3b7610 Fix off-by-one in button checking (fixes artf3861)
dac04cb4a Implement Joystick Outputs and Rumble (fixes artf3807)
7d026be26 Various getters and setters added to C++. usleep added to the getters that require a little time for solicted response (getPIDF, getIzone, and getFirmwareVers. Tested against the TALON SRX unit test originally written for CanTalonSrx HAL class.
5893d28f3 Added support for basic PID in java Talon SRX.
cd0194590 Image v20
36c53667c Require Jaguar version v108 or higher.
ea610eb30 Getters for : AppliedThrotte, CloseLoopErr, Sensor/Ain/Enc Pos and Vel are now signed extended.  Before this negative values would not de-serialize correctly. There are some redundant TALON fixes in this particular commit, hopefully it handles ok on Jenkins. Tested with Talon SRX Unit Test (firm 0.34)
d04476bb2 Wired close loop pos and vel to CANTalon::Set(). Did basic testing with close loop pos with talon slaving.
2bb0a32c1 Change Periodic Status rate to 20ms. Jaguar firmware v109 fixes issue with periodic status sending.
cdbe80315 Image v19 This updates the hal headers and ni libraries for image v19. There were very few changes this time around, only some network communications stuff. Also updated the minimum version number in the build properties to the new image version
517e708fd Merge "Reorded network table init to avoid error and added disabledInit to template (artf3841, artf3840)"
70825be69 Reorded network table init to avoid error and added disabledInit to template (artf3841, artf3840)
43532198c Merge "Artifact artf3520 : Need a PDP Clear Sticky Faults API"
4da9ebe1f Artifact artf3520 : Need a PDP Clear Sticky Faults API
9479793e1 Merge "Open scope of several data fields to be able to extend CommandGroup"
20e9f499b Merge "Add hasPeriodPassed function to java, for parity with C++ Timer API"
3d897cef5 Make robot programs deploy as lvuser for correct permissions (artf3860)
fa229f2b1 Added java joystick message spam fix (artf3836)
b1056cf6d Prevented missing joystick messages from coming out more than once a seccond (fixes artf3836)
8e707169a Add hasPeriodPassed function to java, for parity with C++ Timer API
47961c8b1 Open scope of several data fields to be able to extend CommandGroup
b59f4141c Merge "Open scope for gyro methods. Fixes FirstForge artf1699."
d62d82b28 Merge "Feed motor safety when StopMotor is called. Fixes artf1687 on FirstForge."
a9d30c038 Fixed a typo in the SRX sample project to correct a variable name error
6e0c84d94 Feed motor safety when StopMotor is called. Fixes artf1687 on FirstForge.
bb8ea17ac Open scope for gyro methods. Fixes FirstForge artf1699.
c683e24aa Merge changes If51bf61d,Ia6f4997b
7b371f6d7 Added Omar's new CanTalonSRX code.
f6de7bc96 Merge "Java debug launcher no longer waits for text marker to appear in console."
c00d9f152 Updated the names and descriptions for the c++ vision examples
6ec2d262c Added nicer Java interface for Talon SRX -- throttle mode works.
3c8b31608 Merge "Use standard eclipse dialogs instead of Swing dialogs"
29f36b0ea Merge "Overwrite instead of append during version check. Fixes artf3818"
486885e8b Merge changes Id7a97940,I6234fe06
020d97580 Merge "Fixed bug with SDFormat version changing."
28a41e4ac Added support for CAN Talon SRX in C++ and Java.
f590cda8f Merge "match templates with robot builder."
c98f54dbb match templates with robot builder.
81840b2c4 Add a getControlWord() call to the run thread in DS so that the program is kept alive
78ccb48fd Java debug launcher no longer waits for text marker to appear in console.
b9913d3e1 Overwrite instead of append during version check. Fixes artf3818
56430ccd7 Merge "Pipe java program output through NetConsole"
d412584f1 Merge "Tried to improve reliability of a couple of unit tests."
364a3afad Pipe java program output through NetConsole
97456f40f Don't buffer NetComms data and add IsDSAttached() check to C++ IsEnabled() method (fixes artf3747)
7e5ed03d2 Check if Joystick Button exists when requested and pass 0 and warn if it doesn't
14a1e6ae8 Get MatchTime from NetComms (fixes artf2538)
529f5b773 Use standard eclipse dialogs instead of Swing dialogs
91c70daf5 Tried to improve reliability of a couple of unit tests.
e86312cd6 Fixed bug with SDFormat version changing.
99dc3c90e Merge "Removed the tail command"
4ad8818a8 Added vision samples
e4babbe4d Removed the tail command
9ec516407 Merge "Handle cases where no DS is attached (initial m_controlWord, reportError, getAlliance and getLocation) fixes artf3778"
033e5f37b Merge "Add methods for checking Watchdog status, ds status, and brownout status"
e3d256e44 Merge "Change uint to int for channels on constructors which can take a single channel or single pointer to avoid ambiguous calls."
867e4080d Change uint to int for channels on constructors which can take a single channel or single pointer to avoid ambiguous calls.
4be9732e9 Moves C++ global build properties to properties file to match Java. Adds host reachability check and static IP fallback Adds roboRIO Image check Adds JRE check for Java
cd29e1c32 Handle cases where no DS is attached (initial m_controlWord, reportError, getAlliance and getLocation) fixes artf3778
e73b3ed7b Add methods for checking Watchdog status, ds status, and brownout status
d7a979408 Simulator makefiles: Set file extension based on platform
6234fe06f Fix CMakeLists.txt to not be platform specific
c7a90b2cc Merge "Fix write when used with long byte array."
e5443f0e7 Merge "Added in new headers and libraries for image version 18. This image contains a change to fix artf3773, which switched setDataSem to be a pthread_cond_t variable instead of a mutex. As a result, a few new HAL functions had to be exposed over JNI, specifically the functions for MultiWait."
0abe19a1a Merge "Added support for launching a simulation friendly SmartDashboard."
ccd64090b Fix write when used with long byte array.
c6891fc03 Added in new headers and libraries for image version 18. This image contains a change to fix artf3773, which switched setDataSem to be a pthread_cond_t variable instead of a mutex. As a result, a few new HAL functions had to be exposed over JNI, specifically the functions for MultiWait.
6f4d6ed99 Add support for vision in C++
0670ff145 Merge "Using netconsole-host to start programs and get output directed to netconsole and the log file (fixes artf3777, artf3750)"
655ade643 Using netconsole-host to start programs and get output directed to netconsole and the log file (fixes artf3777, artf3750)
7cfa0d04d Added support for launching a simulation friendly SmartDashboard.
f1476be27 Reverted accidental commits
9be6ee471 CANTalon throttle works.
9050ea7e3 Generalize CANJaguar and CANTalon with a CANSpeedController interface in C++
c1f68eb2b Stick the CanTalonSrx code in, get it to compile
fb53eea6b Merge "[artf3749] Repaired undefined behavior in takeMultiWait."
6c294e137 Increase timeout on network table test
988defee1 Merge "Adds the new shared object libraries and headers for image version 17"
279ae1cd9 Merge "Removed remains of the old toolchain method."
49f6b90d1 Merge "Removed the timing option from Iterative Robot Template since it conflicted with its purpose (artf3751)"
6cd5eeab6 Adds the new shared object libraries and headers for image version 17
c883c9e2f Removed release configuration from template (fixes artf3688)
54951c888 Removed remains of the old toolchain method.
739508ff8 Merge "First cut at pulling in riolog plugin."
f67849a9b [artf3749] Repaired undefined behavior in takeMultiWait.
7c5b3c628 Merge "Added the OutlineViewer to distribute with the plugins."
eb536ba34 Merge "Added default world files to examples."
19737ba44 Merge "Implement user API for roboRIO power methods (fixes artf3728 and artf3537)"
9f0bed2e6 Merge "Correct off-by-one error in DigOut PWM Gen B HAL code (fixes artf3705 for C++/Java)"
86e4c3b77 Merge "athena-deploy now calls the clean step to deal with a known ant issue where it does not detect changed constants. This addresses art3766."
e216d1abd Merge "Fix all the samples to build with the C++11/14 language features (constexpr)"
20aeb5c6c Added the OutlineViewer to distribute with the plugins.
3ad31dd4d Implement user API for roboRIO power methods (fixes artf3728 and artf3537)
4c5b8c8c4 Correct off-by-one error in DigOut PWM Gen B HAL code (fixes artf3705 for C++/Java)
747cdc8a5 Updated an incorrect voltage constant for the 5V input rail, and added parenthesis to make the order of operations explicit. Fix for artf3728
c3c7baa15 Removed the timing option from Iterative Robot Template since it conflicted with its purpose (artf3751)
2fdb7c8eb athena-deploy now calls the clean step to deal with a known ant issue where it does not detect changed constants. This addresses art3766.
1d0a6b8ac Fix all the samples to build with the C++11/14 language features (constexpr)
08fdf45f6 Added default world files to examples.
733847119 First cut at pulling in riolog plugin.
da0cc0c83 Merge "Fixes for 0 based joysticks and joystick axes in simulation."
6e0637e77 Merge "Upped the limits for the output on some PID tests."
8d49d8255 Merge "Move SerialPort to HAL and add SerialPort support for Java Squashed commit of the following:"
3b53f84c8 Fixes for 0 based joysticks and joystick axes in simulation.
05bb3cdd7 Upped the limits for the output on some PID tests.
18de3aebd Make sure the whole JVM exits if an exception is thrown. This prevents the DS thread, NT thread or any additional user thread from keeping the program alive when the main thread exits (fixes artf3743)
de5e5ab40 Merge "[artf3709] Fixed PIDController loop timing."
6b6e5d953 Move SerialPort to HAL and add SerialPort support for Java Squashed commit of the following:
fa20e6ca4 Check for fatal interrupt status on multiple interrupt methods to avoid hanging program (fixes artf3602)
e56aa87af Merge "Change Talon to use 1x update rate (fixes artf3733)"
63bb8e1f3 Merge "Remove unimplemented sendErrorStreamToDriverStation method (fixes artf3617)"
c1e63acc1 Merge "Use ByteBuffer for conversion from raw sensor bytes to Java types (fixes artf2673)"
68ba9bfea Merge "Detect projects which are duplicates on Windows due to casing (fixes artf3487)."
aa643d8ba Merge "Update Interrupt Javadocs (fixes artf3492 and artf3602)"
1f8a3d5bf Merge "DigitalInputs are now automatically added the LiveWindow in both C++ and Java"
2fa04c175 Merge "Don't raise exception for Joystick axis/POV out of active range. Shorten error message and change to Warning (filtered by DS by default)"
cd3db9631 Merge "Move user program to home\lvuser based on comments from Joe"
8ccb99d87 Merge "Added ignore support for intellij"
b14ca0847 Merge "[artf3717] Added isEnabled to Teleop Loops in Samples."
b898cec90 Change Talon to use 1x update rate (fixes artf3733)
687e2c671 [artf3709] Fixed PIDController loop timing.
594d0d802 Use ByteBuffer for conversion from raw sensor bytes to Java types (fixes artf2673)
0f825b717 Added ignore support for intellij
345ceafa0 DigitalInputs are now automatically added the LiveWindow in both C++ and Java
b030be68d Detect projects which are duplicates on Windows due to casing (fixes artf3487).
fba155804 Update Interrupt Javadocs (fixes artf3492 and artf3602)
97f954aef Remove unimplemented sendErrorStreamToDriverStation method (fixes artf3617)
80194e980 Merge "Added message to notify when simulation is unsupported."
767686ae2 [artf3717] Added isEnabled to Teleop Loops in Samples.
7f30b6bff Don't raise exception for Joystick axis/POV out of active range. Shorten error message and change to Warning (filtered by DS by default)
8c395ca29 Move user program to home\lvuser based on comments from Joe
5b2520c35 Merge "Fixed building of the frcsim-libwpilibsim-cpp deb."
fa8d7b843 Fixed building of the frcsim-libwpilibsim-cpp deb.
93100c0e6 Merge "Increases the runtime of the PID test to improve stability."
b80565d06 Merge "Decreases the amount of console spam during tests."
2d45cda74 Increases the runtime of the PID test to improve stability.
e4d90c6cb Decreases the amount of console spam during tests.
18bdb7fde Merge "Added C++ Encoder samples to Eclipse Plugin."
f6e8db50f Merge "Fixed New Project Wizard to have a radio option always selected"
20c60a5fe Added C++ Encoder samples to Eclipse Plugin.
66b7a8b66 Merge "Use either DO PWM A or B not A and B and update javadoc to reflect 6 generators (artf3698)"
8ebf88b74 Java DriverStation.InTest: Set correct flag.
b85b0c8eb Merge "Keep Notifier firing after FPGA rollover (fixes artf3582), simulation may still have an issue with counter rollover"
c58fde19f Increase the number of joystick ports to 6.
003dc0dc2 Pass errors to DS in C++ and Java
26419ec20 Don't use "final" in command-based template RobotMap.
383439369 Merge "Improves the general stability of the CANJaguarDefault tests"
97be9765c Changed the version to include time to fix issue with replacing.
0881ac0b2 Fixed issue where plugins don't show up on some installs.
1b7e5970f Merge "Added C++ sample for CAN monitoring of PDP."
bc84c7ac7 Improves the general stability of the CANJaguarDefault tests
fc34b9b80 Merge "Aggregate javadocs and use JavaFinal docs when building plugins (fixes artf3533)"
9af070a4a Added C++ sample for CAN monitoring of PDP.
93925bc02 Fixes the GyroTest
132804a4c Updates the test scripts to prevent a race time condition
9e6d68367 Merge "Increases tolerances for the PID tests"
f7e8b95a2 Increases tolerances for the PID tests
213e73c43 Merge "v16 Image"
455654a95 Merge "Fix joystick issues [artf3707]"
261b1857c make getControlMode public. Was public in 2014.
f3fe64a60 Keep Notifier firing after FPGA rollover (fixes artf3582), simulation may still have an issue with counter rollover
f108853e1 Merge "Added sample programs for Solenoids and Relays."
a56c0eb4a Fix joystick issues [artf3707]
8c01e9065 Fixes a bug with the test stand launch script
be30d3ab1 Fix CANJaguar brownout recovery in Java
53255a764 Merge "Relay: Allow "On" state to be set via SmartDashboard."
3abd352be Added sample programs for Solenoids and Relays.
70be534fe v16 Image
e677d52a2 Increases the allowed runtime for two network tables tests
62790c0f4 Use either DO PWM A or B not A and B and update javadoc to reflect 6 generators (artf3698)
cd75dc71e Merge "Recover from signals in Wait() [artf3495]"
a381b60e3 Merge "Added sample program for Motor Controller."
311a69026 Added sample program for Motor Controller.
a2f015c25 Merge "Dynamically generates tools zip file."
5c26b37d3 Dynamically generates tools zip file.
8dba364be Recover from signals in Wait() [artf3495]
76f9b3bbc Merge "Adding stdint.h to the Joystick.h makes the indexer work (fixes artf3505)"
bbeb0084a Merge "Set RobotBase instance in constructor [artf3652]"
e51c3daf0 Set RobotBase instance in constructor [artf3652]
dc89ec673 Adding stdint.h to the Joystick.h makes the indexer work (fixes artf3505)
fc7c01582 Fix warnings for javadocs
c52e29432 Merge "make preferences check for [ and ] in key"
53d50f470 Merge "updted licesne strings in eclipse plugins"
ca303e016 Merge "fixed  template on checkedin java examples"
042a1dbc8 make preferences check for [ and ] in key
3c91de56d Merge "Fix segfault in DIO PWM generators [artf3653]"
0c2f3c19c Merge "Fix Java Joystick.getPOV()."
023d20abc Fix segfault in DIO PWM generators [artf3653]
5c702edd8 Support for pushing NetworkTables (java) to the maven repository.
d90bbc195 Merge "Removed vestigial POM file from NetworkTables"
3365d0dc1 Fix Java Joystick.getPOV().
615d62a21 Relay: Allow "On" state to be set via SmartDashboard.
78988584e Removed vestigial POM file from NetworkTables
294e224fc Merge "allow installation on eclipse 3.8 (ubuntu 14.04)"
436c92961 Merge "Add new joystick features"
341abbfaa Merge "Run the scheduler in disabled (fixes artf3631)"
eab34bbb8 Merge "Add SD methods with default values to match Java (fixes artf3648)"
5db532079 Merge "Add method to get Device ID to Can Jaguar (fixes artf3613)"
54eaa913f Merge "Make Java ignore section headers in Preferences file and start file with [Preferences] to match C++ (fixes artf3629)"
8a541a67c Add new joystick features
7fa0eea49 Merge "ADXL345_SPI: Report usage."
cc431354b Merge "Counter::setUpSource(AnalogTrigger): actually set the source."
65c10cd46 Aggregate javadocs and use JavaFinal docs when building plugins (fixes artf3533)
fea52a77a Add SD methods with default values to match Java (fixes artf3648)
4e31b6800 Add method to get Device ID to Can Jaguar (fixes artf3613)
f99b57b1f Run the scheduler in disabled (fixes artf3631)
dad3b68f3 Make Java ignore section headers in Preferences file and start file with [Preferences] to match C++ (fixes artf3629)
08c872317 Merge "Add error message to new project wizard if project already exists"
ce768ae34 ADXL345_SPI: Report usage.
217c50c28 allow installation on eclipse 3.8 (ubuntu 14.04)
fcc8230db Counter::setUpSource(AnalogTrigger): actually set the source.
d2c1bc174 updted licesne strings in eclipse plugins
6fe638e97 Fixed New Project Wizard to have a radio option always selected
07837c4ab fixed  template on checkedin java examples
daee45780 Added additional NI libraries to make C++ builds work
807948282 Create HOME on windows
0b828eec6 Merge "Disable CANJaguar current PID test"
49e06bd13 Merge "Move interrupt methods to InterruptableSensorBase"
eca4d36a4 Disable CANJaguar current PID test
4d2a720f4 Move interrupt methods to InterruptableSensorBase
591e1de72 use WPILIB eclipse var & use luna to build
dc09233fc Merge "void* -> void by creating proxy fuction"
037d3b2fb void* -> void by creating proxy fuction
a3995a202 Fix bitfield indexing on PWM Latch
a71f07ed6 Merge "HAL: build shared library as well as static library."
cb81aa6fe Merge "Update headers and .sos to v15 image + most API changes"
ace43a6a1 Merge "DigitalOutput: Fix enablePWM()."
a4e8d65b7 Merge "Relay: In free(), set both forward and reverse to 0."
bfd88f1da Reserve network communications
a65cb500f Relay: In free(), set both forward and reverse to 0.
f27df43b5 DigitalOutput: Fix enablePWM().
7e9f183cf Update headers and .sos to v15 image + most API changes
2f2636139 Added Mecanum drive sample program
52ecbd38a HAL: build shared library as well as static library.
6089722c4 Merge "Too many bugs with LTO in GCC 4.9 & Binutils 2.24. Disabling. Maybe later."
244ecd23e Too many bugs with LTO in GCC 4.9 & Binutils 2.24. Disabling. Maybe later.
8aeacac30 Set Start Mode to remote to allow Debug As->WPILib Deploy to launch without error on Windows
45021c7f3 Merge "Figured out the madness: not launching properly (no copy)"
857a1abb0 Merge "Prevent leading zeroes in team number and warn user about them (fixes artf3429)"
c11f194cb Merge "Allow dataRecieved to be null in I2C transaction (fixes artf3603)"
8b806e11e Added Arcade C++ example and a hierarchy for C++ examples.
ba5111b99 Add a Motor Controller example java program
88bf4ee56 Merge "Added a hierarchy of example categories that looks very much like what's in LabVIEW and placed the one project in a bunch of categories as a proof of concept."
76295a574 Fix eclipse launching of tools (SmartDashboard, sfx, OutlineViewer, and RobotBuilder)
90a106ccf Added a hierarchy of example categories that looks very much like what's in LabVIEW and placed the one project in a bunch of categories as a proof of concept.
0fcef9490 Whoops, need to change the joystick port assignments to be zero based on the sample program.
3f38f3e84 Added a tank drive example for java
64abdb822 Allow dataRecieved to be null in I2C transaction (fixes artf3603)
aaa7eabd0 Prevent leading zeroes in team number and warn user about them (fixes artf3429)
1cef27134 Changed joystick port numbers to be zero-based for C++ and Java.
aaab77cdb Figured out the madness: not launching properly (no copy)
7e2c68214 Set debug flag user to lvuser to allow it to be deleted (fixes artf3415)
ad906da67 Merge "Squashed commit of the following:"
a6ea51ad0 Fix simulator builds (and don't clean, dh does that also)
d72ee199f Squashed commit of the following:
19b2d0523 Fix simulator builds (dh knows how to CMake)
26d789b89 Copy to the correct folder name
f50b05845 Merge changes I6b5d2767,I6f4b94d1,Iada981fc
39da9d67b JNI should ignore -lfrc and installing the new ld scripts
b6475d050 Renaming and merging WPILibC++ into Devices and simulator files
295f212c8 Change digital input to return boolean values to match Java
fa4a39dcc Initial linker script to abstract away the massive library list
66c653a21 Get the simulator build to accept c++11 languge
340a3d492 Fixing pedantics and using RTTI for command name
6710ac3a2 fixing all warnings and making sure they will stay fixed
9e62e60e6 Merge "Remove CPP Preferences page from WPILib preferences (code has already been deleted)"
e6c56e444 Modified simulation debs to exclude building the toolchain.
35f1aded6 Merge "Change RobotState functions to return actual values (this time with nullptrs) [artf3611]"
4904e1dc2 Change RobotState functions to return actual values (this time with nullptrs) [artf3611]
28b9c18e2 Merge "Add a getDeviceNumber method to CANJaguar"
a548b14b7 Add a getDeviceNumber method to CANJaguar
323022acf Remove "using namespace std;" from headers
1bcdd301e Merge "Fix changes where the default value was always being returned for many of the robot state status functions."
21f728c80 Moving to arm-frc-linux-gnueabi in cproject and attempting to use gdb (overwritten)
49d440ecc Merge changes I4f5a36e6,I248d27cc,I55769599,I7688b8cd
00486e2ec Fix changes where the default value was always being returned for many of the robot state status functions.
a3115dac1 Removing Toolchains!
1f35670a1 Enabling C++14 and removing path for new toolchains
d1d81e5e1 Remove unnecessary config for cproject/Simulate target
a7b166e3f Remove CPP Preferences page from WPILib preferences (code has already been deleted)
6c28e0966 Updated bootstrap file to install the new toolchains
8199fb28d Updating template .cproject to avoid specifiyng commands and defaults Fixes unknown GCC errors (supposedly)
f2920fd77 Enable C++14 and Wextra
ebd9667ba Expose and fix error where old subsystems were being referended after use in tests
3f0f7931b Changing cmake to test the tests (Jenkins)
cad83ed3c Bump the version number for the windows toolchains to force them to reload.
62d657942 Add missing semi-colon to cproject file
1d7b17a2b Merge "Add quotes around linker flag path to libs (fixes artf2390 / artf3453)"
738859c4e Fixed bug where mac toolchains always reinstall on Eclipse start
127ff0ac0 Fixing extraction recursion: use provided version
472f51fc7 Add version=current to global wpilib.properties file on any property save
887a4e94f Add error message to new project wizard if project already exists
0f0850ca9 Merge "Update to v14 headers and libraries"
ab27f795b Update to v14 headers and libraries
b16a03775 Store preferences in a directory writable by lvuser
1e812ac4d Added comments to the compressor class per artf3527.
ecc6815f6 Merge "Fix status buffer allocation in SolenoidBase"
ca9f5a676 Merge "Make all channel errors throw IndexOutOfBounds"
debf9e0ee Make all channel errors throw IndexOutOfBounds
15abbb36c Fix status buffer allocation in SolenoidBase
d9b974300 Add quotes around linker flag path to libs (fixes artf2390 / artf3453)
e60baf41a Added the "version=current" property to allow projects to correctly reference the updated installation
09cb3a22c Merge "Change the tail command for capturing the logs on deployment to check the file every second instead of continuously to avoid the 100% CPU time problem from artf3524"
27ecd3583 Change the tail command for capturing the logs on deployment to check the file every second instead of continuously to avoid the 100% CPU time problem from artf3524
ff6d18030 Revert "Fixing java version number to be current"
410b739c2 Fixing java version number to be current
2144b853d Set the cpp-version variable per-project
f87c517e6 Merge "Remove Version number from zips"
77dac9bd7 Remove Version number from zips
56cf287b3 Merge "require pthread and rt lib for Itests"
dc970d9a6 require pthread and rt lib for Itests
da2ea9ea8 remove/fix references to cRIO documentation. Edited comments related to sidecar, breakout, 9472. Some references were to unused methods in simulation that were removed.
b1ace7937 Fixes the MotorEncoderTest sometimes failing because a counter registers movement from the jaguar after reset.
2a8ade093 Adds the ability to run test classes with  --repeat rule.
b7458e7ba Merge "Make Talon the default speed controller for RobotDrive"
3fddee51c Make Talon the default speed controller for RobotDrive
21967fcb2 Merge "Fixes testing framework typos & adds rule to keep unix line endings for .sh"
9b896eab7 Make DoubleSolenoid work in Java [artf3457]
f439adc2a Merge "fix for Artifact artf3431 : Solenoids initialize to random/arbitrary stat"
67cb85467 Fixes testing framework typos & adds rule to keep unix line endings for .sh
ef7c55402 Merge "Wrap CPP Lib and Include paths in quotes (fixes artf2390)"
359b4ec86 Merge "Added support for 32-bit builds."
21153fc63 Merge "Fix artf3476 move RobotState and HLReporting implementation set to somewhere they will get called"
daa8bd67e Added support for 32-bit builds.
e5e568fa7 Wrap CPP Lib and Include paths in quotes (fixes artf2390)
c9f6fcd96 Fix artf3447 Java only runs teleop - Correct off by one error in Control Word compares
778620b0a Fix artf3476 move RobotState and HLReporting implementation set to somewhere they will get called
d58c6f0c0 Merge "Kill program before debugging C++ so file upload doesn't fail"
a14d85421 Cpp tests without color for jenkins. Fixes startDS to not kill the driver station.
c5fc1dc5b Merge "Change SampleRobot template to use 0 based PWM"
cded5351d Test scripts have permission to run and fixes sshpass.
47169e0ce Adds test scripts to deploy, run and retrieve results from integration tests.
4ef798a0c Merge "Updates gitignore to use a file generated using http://www.gitignore.io"
7ec8d4e9a Updates gitignore to use a file generated using http://www.gitignore.io
17e7fc8f2 fix for Artifact artf3431 : Solenoids initialize to random/arbitrary stat
023f955a5 Change SampleRobot template to use 0 based PWM
52e358c18 Merge "Add Preferences to simulation"
7638e2b6e Kill program before debugging C++ so file upload doesn't fail
f9ab84d91 Integration tests now run with the Ant Junit test framework.
04fadb85f Add Preferences to simulation
698b371b1 Merge "Add OI and RobotMap files to Command Project template (fixes artf2407)"
6dde2c2b4 Merge "Adds interrupts to Java"
8ba0eada1 Adds interrupts to Java
bf5eaf657 Add OI and RobotMap files to Command Project template (fixes artf2407)
3baaea5a4 Squashed commit of the following:
2cc937f5e Fixes debugging in Java
23b6a980c Merge "Uses ${ant.java.version} for javac when compiling"
89b80db2d Merge "Fix PIDError property"
1f27d3688 Fix PIDError property
125dfac8b Merge "Fix Java IT build"
4215d1c79 Fix Java IT build
dddba82d1 Fix artf3337 - add include to define int32_t in CounterBase.h
78e2a8d1b Uses ${ant.java.version} for javac when compiling
687bc44ae Add new CTRE classes and update PDP tests
c82a94b26 Merge "Make Java tests run with TestNG"
d407fe546 Merge "Fixes for simulation parallel builds."
699949fdf Fixes for simulation parallel builds.
6d8997401 Make Java tests run with TestNG
e52315750 Merge "Fixed bug with joystick buttons/axes out of range."
3eba1c468 Merge "Add delays to make C++ ITs more reliable"
d466d17ed Add delays to make C++ ITs more reliable
9dfd6bde9 Adds a Vagrant configuration file and setup script
eded28ebf Fixed bug with joystick buttons/axes out of range.
c68ee3001 Added message to notify when simulation is unsupported.
777dc2113 Merge "Updated installer to point to the new location."
174f2c0e2 Merge "Increased build speed + caching of debs."
434caf81d Merge "Add back SampleRobot Template Add GettingStarted example based on IterativeRobot"
932717006 Merge "Beginning of gradle conversion."
597e209c0 Add back SampleRobot Template Add GettingStarted example based on IterativeRobot
ad1ace102 Increased build speed + caching of debs.
70d09de2e Fixed frcsim script to have the proper LOAD_LIBRARY_PATH.
f7e1753e0 Fix C++ linker settings
df3f3cc0e Updated installer to point to the new location.
25308e244 Beginning of gradle conversion.
6b3e2690d Fixed placement of libWPILibAthena.a
ae10d4390 Fixes for examples and Gazebo 3.1.
d4e377fc0 Merge "Adds the Trigger template and adds WPILib.h import to all templates"
9126646fe Adds the Trigger template and adds WPILib.h import to all templates
d3f632486 Merge "Added support for Jenkins to generate doxygen."
268459206 Added support for Jenkins to generate doxygen.
df390c275 Merge "Merged sources in wpilibJavaFinal."
1742634a9 Merge "Fix the example programs"
0f8f83500 Merged sources in wpilibJavaFinal.
d5c73c95d Fix the example programs
ca7dc5d6a Removed excessive linking.
66e1f2a18 Fixed a few bugs with C++ due to the merge.
1301d76d6 Fixes for HLUsageReporting due to Jonathan's exceptions.
a72ea14f9 Removed Simple Robot template option
14b2f1466 Merge "Implement DriverStation::GetBatteryVoltage"
d2cd5f357 Implement DriverStation::GetBatteryVoltage
7ecde3a33 Merge "Fix the default C++ linker settings"
81143fc53 Merge "Adds Exception throwing the basic robot systems are not properly initialized."
c58a86a3a Fix the default C++ linker settings
2f655bb49 Merge "Fixed bug with plugins not including all simulation dependencies."
657054c9e Fixed bug with plugins not including all simulation dependencies.
f4d542b21 Add FakeEncoderTest for C++
8db11a4c6 Fix to deal with the certificates on collabnet having expired.
14d784bca Fix import, resolves artf3339
7c8124d76 Allowed sharing of common C++ code between RoboRIO and Simulation.
b371600f0 Merge "Completed artf2675 - rename SimpleRobot -> SampleRobot."
6c3b002a8 Merge "Refactors the CANJaguar tests to be more straightforward."
0f9324764 Completed artf2675 - rename SimpleRobot -> SampleRobot.
54439e719 Refactors the CANJaguar tests to be more straightforward.
5b8279f40 Remove old driver station code
8abbcf53f Update to the v13 headers and libraries
b8eeeabbb Merge "Removes encoder.start() from the example programs"
9a2bd8c49 Removes encoder.start() from the example programs
4db634b34 Fix PCM channel indexes
57e670c18 Merge "Move PWM allocation to HAL to allow for checking against DIO allocation Re-number MXP DIO to match pinout (include SPI and I2C pins) (fixes artf2664) Change PWM MXP mapping to accommodate DIO re-mapping This re-implementation also fixes artf2668 for C++ and Java Change the test bench to reflect this change also"
59473ab7a Move PWM allocation to HAL to allow for checking against DIO allocation Re-number MXP DIO to match pinout (include SPI and I2C pins) (fixes artf2664) Change PWM MXP mapping to accommodate DIO re-mapping This re-implementation also fixes artf2668 for C++ and Java Change the test bench to reflect this change also
d1d4c7521 Merge "Fixed interrupt freeing in C++"
2356008d8 Fixed interrupt freeing in C++
deb335d96 Fixed DoubleSolenoid, added a double solenoid test
1ce03b9c4 DoubleSolenoid works in C++ now
a09f75934 The output range can be set on a PIDSubSystem
aab98c063 Adds Exception throwing the basic robot systems are not properly initialized.
741e28b85 Merge "Added version numbers to maven plugins"
f817f6d04 Added version numbers to maven plugins
7442a7ed7 Merge "Only update FRC Java Projects (fixes artf2627)"
765198f5f Merge "Updated some comments that mention the cRIO"
ed08ab298 Fixed voltage range checking and error message
4b22742cd Merge "Cleaned up C++ compiler warnings"
45f93e9ab Merge "Added support for building debs on jenkins."
049377c9e Added support for building debs on jenkins.
f4ace4a36 Cleaned up C++ compiler warnings
9ff420547 Updated some comments that mention the cRIO
1be31431b Merge "Removed the User LED functions"
32cafd0ef Removed the User LED functions
0216d1b33 Merge "Gyro deadband defaults to 0"
43c566bd8 Gyro deadband defaults to 0
b86c74722 Only set up once in the C++ tests
60a3fd069 Added gyro deadbands
b86c11493 Only update FRC Java Projects (fixes artf2627)
14f201cd3 Merge "getAnalogAverageValue should use a 32-bit int"
c49ea255b getAnalogAverageValue should use a 32-bit int
065e1c24e Update plugin menu text and make template menu options only show for correct language (fixes artf2408)
ecdb77e4b Update plugin target to use mDNS (causes Java debug to now use mDNS)
10aad6d1c Merge "Fixed a few more small TODOs"
e73c8d06e Fixed a few more small TODOs
dc341a448 Fixed a few simple SmartDashboard FIXMEs and TODOs
d521eb79b Analog interrupts in C++
f57a2dc5a Merge "Change deploys to use MDNS and to retry tail on missing file (fixes artf2663)"
7006d1ebc Change deploys to use MDNS and to retry tail on missing file (fixes artf2663)
c32e5707d Merge "Remove extra semi-colon"
c27da3587 Merge "Added an interrupt test for C++"
f18cccbc3 Remove extra semi-colon
b91b68143 Added an interrupt test for C++
b26667f86 Fixed the PCM test solenoid numbers
792e3b6cc Removed modules from the HAL and JNI bindings
fd4379a94 Makes the Counter.Mode an enum and adds Null Checking
a40cdf519 Implements the AnalogTriggerType as an enumeration
ba4e74d29 AnalogTrigger support in Java
38583789b Fixes the TiltPanCamera test (now GyroTest)
45e43b627 Merge "Changes the name of the AccelerometerTest to BuiltInAccelerometerTest"
e837d9083 Changes the name of the AccelerometerTest to BuiltInAccelerometerTest
e0e2b498a Merge "Accumulators wait for the next sample after reset"
8fe888dbc Accumulators wait for the next sample after reset
41bb0da4e Merge "Fixes bugs with the Gyro"
97ade3606 Fixes bugs with the Gyro
2735406bf Merge "Added a Java internal accelerometer test"
ab04e19aa Added a Java internal accelerometer test
a58288ae6 Merge "Refactors the RobotBase Setup into a method."
7905259e2 Refactors the RobotBase Setup into a method.
f018689d0 Merge "Completed artf2662: removed Start()/Stop() in Encoders and Counters."
0bb13d86e Completed artf2662: removed Start()/Stop() in Encoders and Counters.
526df3679 Merge "Added a C++ Preferences test"
d8da3e5f1 Added a C++ Preferences test
c0af23505 Removes the dependency-reduced-pom.xml from the repository.
7f6ca6824 Merge "HALInitialize should be the first thing called"
231bb55b2 HALInitialize should be the first thing called
76e488061 Removed cRIO tests, NetBeans stuff, and Ant stuff
5e8ea3846 Merge "Fixes the PDP test."
a5b72d62a Remove the last obsolete interface
a5e15b16f Fixes the PDP test.
264c38a67 Merge "Removed the old "parsing" interfaces"
20de3abe8 Merge "Updates the TimerTest to give a clearer output."
3536d4783 Updates the TimerTest to give a clearer output.
60a294fba Removed the old "parsing" interfaces
dc42a1129 Merge "The LiveWindow instance isn't a global static"
386dc1f16 Added AnalogTrigger tests
038478e43 The LiveWindow instance isn't a global static
26e90a988 Merge "Adds/Updates CANJava Testing Framework."
d5cd47bfa Merge "Minor updates to the C++ CANJaguar ITs"
1e35ef780 Adds/Updates CANJava Testing Framework.
65607b5bc Merge "Fixed CANJaguar::GetOutputVoltage for negative voltages"
30c0cc056 Minor updates to the C++ CANJaguar ITs
92bd69741 Fixes a bug where the testing framework would never set the implementation for the libraries.
ed0df5432 Fixed CANJaguar::GetOutputVoltage for negative voltages
6deb196e9 Support for the "USER" button on the RoboRIO
8b612f713 Merge "CANJaguar::Disable stops periodic setpoints"
d66bafb68 CANJaguar::Disable stops periodic setpoints
78d2ccd29 Merge "Added stack traces and better error reporting in C++"
06d59447b Merge "Adds missing Javadocs and @Overrides annotations to the PIDController in Java"
3475a4a8c Adds missing Javadocs and @Overrides annotations to the PIDController in Java
6af242b55 Merge "Fixes for javadoc eclipse plugin."
c72e70439 Fixes for javadoc eclipse plugin.
89fe909ae Added stack traces and better error reporting in C++
c1d8e4ef4 Make AnalogInput PIDGet return an average voltage. artf2391 for Java. Equivalent to Ia7f06ca2 Previously it returned a raw value instead of a voltage.
338120c3a Merge "Fixed wpi_setErrnoError to print the error name"
a7efbe0d7 Merge "Fixed Preferences in C++"
4c7828030 Merge "Correct voltage range in javadocs for RoboRIO"
5a3889a3a Correct voltage range in javadocs for RoboRIO
2fd4964b6 Merge "ADXL345_I2C: Make constructor explicit."
6ae639454 Merge "AnalogPotentiometer: Make constructors explicit."
310151132 AnalogPotentiometer: Make constructors explicit.
7f4e1e39a ADXL345_I2C: Make constructor explicit.
eebdc3d20 BuiltInAccelerometer: Add virtual destructor.
980ea96b0 Fixed Preferences in C++
1038f98e8 Fixed wpi_setErrnoError to print the error name
326aa2e85 Fixed a typo with the tGlobal commit
6071fc7fb Fixed CANJaguar percent scaling issue [artf2637]
f958b65ba CANJaguar can be disabled [artf2647]
f4f7588cf Merge "A few small changes to the C++ ITs"
80ecff6bd Merge "AnalogInput::PIDGet returns an average voltage[artf2391]"
e487c950e Merge "Only create one tGlobal object"
191e9d5d6 AnalogInput::PIDGet returns an average voltage[artf2391]
40fc8326a Only create one tGlobal object
fdbe750d3 A few small changes to the C++ ITs
3ec797a8c Updates the CANJaguar to free itself before throwing an exception in the constructor. This allows it to be allocated later without throwing an Allocation exception.
78e6cf720 Updates the AbstractComsSetup message to report JUnits multiple exceptions correctly. Adds a simple logger to the AbstractComsSetup that prints messages to the console based on the log level. Replaces all System.out prints with TestBench.out()
2481e98bc Fixes a deallocation of the Relay resource when calling free. Changes fake "Enum Classes" into real Enumerations
c81d510eb CANJaguar::ChangeControlMode marks the controlmode as unverified
3fb415910 Merge "Added missing call to m_ds.waitForData()"
2e1bd171a Added missing call to m_ds.waitForData()
25e7a077c Merge "Implemented FRCSim artf2628, fixed bugs in non-sim Relay.java and sim PWM.cpp."
dc48dc7f7 Implemented FRCSim artf2628, fixed bugs in non-sim Relay.java and sim PWM.cpp.
2dd45c3ea Fixed FRCSim artf2619, and misc reformatting.
461e35948 Merge "Remove the Kinect code from C++"
78dac49cf Remove the Kinect code from C++
66ba9a728 Fixed some bugs with CANJaguar verification in Java
c3d1e80a6 Merge "Fixed some bugs with CANJaguar verification in C++"
01ca19f78 Merge "Added a Jaguar brownout test in C++, cleaned up some C++ tests"
202bfb295 Added a Jaguar brownout test in C++, cleaned up some C++ tests
b0369342e Fixed some bugs with CANJaguar verification in C++
26d101caf Restructure WPILibJ to share code.
e84e0ebab Updates the TestBench to use parameters to run specific test methods or test/suite classes. Updates the test bench to only print "Waiting for enable" on one line with a counter. Updates all SubSuites to extend the AbstractTestSuite class. Also includes a small set of tests to prove the validity of the base AbstractTestSuite
0704a697c Merge "Updated the C++ TiltPanCameraTest"
13f97bb6e Merge "Added an Accelerometer interface"
5eddb69aa Updated the C++ TiltPanCameraTest
41c2b9402 Added an Accelerometer interface
99632e003 Merge "Fixed the Command-Based Robot template [artf2550]"
fe12394c9 Fixed the Command-Based Robot template [artf2550]
fbf196763 Merge "Removed AnalogModule, DigitalModule, and Module from C++"
b5fb35c0c Removed AnalogModule, DigitalModule, and Module from Java
48e8b2136 Merge "Add support for downloading models hosted on FIRSTForge."
e962c770b Merge "Fixed installation of frc_gazebo_plugins and a few minor fixes."
1b7a352cb Add support for downloading models hosted on FIRSTForge.
1a77cea13 Removed AnalogModule, DigitalModule, and Module from C++
afa39deec Fixed installation of frc_gazebo_plugins and a few minor fixes.
f27e16735 Adds resource tracking to CANJaguar in C++
d8a5ced01 Merge "Fixes a bug with ErrorBase where the correct error code would not be set when using wpi_setWPIErrorWithContext()"
8fe606a4b Fixes a bug with ErrorBase where the correct error code would not be set when using wpi_setWPIErrorWithContext()
6053a0cc2 Added BuiltInAccelerometer in Java and updated C++
be106b352 Merge "Added a C++ built-in accelerometer class"
ebaf2ef05 Merge "Added HAL methods for using the built-in accelerometer"
9f1a9a07c Added a C++ built-in accelerometer class
ec2a455bc Added HAL methods for using the built-in accelerometer
3d740a9a2 Adds Resource tracking to CANJaguar
1dd1e0be1 Merge "Adds a Unit Test for the Resource object in Java. Fixes a bug in the Resource class caused by allocating a negative resource value."
437e3ff26 Adds a Unit Test for the Resource object in Java. Fixes a bug in the Resource class caused by allocating a negative resource value.
f373c8708 Merge "The camera fixture tests now include an SPI accelerometer test"
54a657a7d The camera fixture tests now include an SPI accelerometer test
60d8508a6 Fix artf2636 Don't consume ByteBuffer when checking class specific status
02a28c8f0 Add allocation checking for I2C MXP. Update DIO counts in Lib layer until resource checking moves down to HAL
b12882897 Update Digital Pin count to reflect full 16 DIO on MXP
1a1a12316 Remove JNA hack no longer being used
d0fdb3e70 Merge "Port SPI to roboRIO. Java SPIDevice renamed to SPI and rewritten to match C++ API."
5d2e20eae Merge "More CANJaguar integration tests"
fc0eb4e95 Adds/updates the documentation for the CANJaguar Classes for C++ & Java. Also removes private unused methods in Java and an unused constructor.
968b69d37 More CANJaguar integration tests
343c7f4f3 Port SPI to roboRIO. Java SPIDevice renamed to SPI and rewritten to match C++ API.
80c5c09f7 Merge "Encoders and counters work on the MXP"
cb9df310d Encoders and counters work on the MXP
f566c087d Fix a few wrong messages in CANJaguar
7ca1b498e Merge "Added generic CAN methods to the HAL"
8bba58b9a Added generic CAN methods to the HAL
8ae64a12e Removed modules from the simulation infrastructure and refactored FRCPlugin.
3b4718fc9 Merge "Fixed FRCSim artf2609 - double ports handled wrong."
5800af49b Merge "Fixed FRCSim artf2599."
63fc4f6cf Merge "Makes the tests take parameters at runtime so that you can selectively run a suite without having to run the entire framework."
8b770ffb4 Makes the tests take parameters at runtime so that you can selectively run a suite without having to run the entire framework.
55fde6b61 Fixed FRCSim artf2609 - double ports handled wrong.
5ddacb43c Fixed FRCSim artf2599.
65c3c0ba0 Merge "Updated the HAL, wpilibj, and wpilibc for PCM and PDP"
fc3ed33f7 Merge "PDP and PCM updated to rely on CtreCanNode parent class, which uses new CAN API. CtreCanNode registers the periodic tx messages and provides an rx function to child classes for easy getters and setters. Some template magic to make the PDP and PCM getters/setters easy to stamp out."
255a3a5b1 Updated the HAL, wpilibj, and wpilibc for PCM and PDP
0ef5c3adf PDP and PCM updated to rely on CtreCanNode parent class, which uses new CAN API. CtreCanNode registers the periodic tx messages and provides an rx function to child classes for easy getters and setters. Some template magic to make the PDP and PCM getters/setters easy to stamp out.
37ebcabc4 Merge "Fixed periodic voltage status message"
244ee8d92 Merge "Fixed C++ deploy in Eclipse"
b97d2eb0c Fixed periodic voltage status message
ae8d22b0f Fixed C++ deploy in Eclipse
ff8016c08 Merge "Fixes CounterTest for C++"
f0fb3023a Merge "CANJaguar uses periodic status updates [artf2621]"
1d33edffe Fixes CounterTest for C++
5bd546f1f CANJaguar uses periodic status updates [artf2621]
d8003899f Merge "Fixed FRCSim artf2615: added more thorough cleaning system to Makefile."
f7bb5cd8f Merge "Re-tuned PID for PacGoat."
4f2d1d9d3 Fixed FRCSim artf2615: added more thorough cleaning system to Makefile.
981f941ca Changed .gitattributes to keep Unix line endings in the robot scripts
5ca2506a7 Merge "Fixed Java deploy script"
25f65a158 Merge "Java Ant script now works with run-at-startup"
15212967e Fixed Java deploy script
2aa030170 comment change
455506976 Merge "Made Java deploy work with run-at-startup"
ef5729b3d Made Java deploy work with run-at-startup
256b052a5 Java Ant script now works with run-at-startup
56cf73168 Re-tuned PID for PacGoat.
0926fa493 Merge changes I7e8735d9,Ida66361c
4de246876 Adds/Formats the CANJaguar.set() Javadocs
130485d76 Adds CANJaguar integration tests testing that in every control mode the motors can be rotated Adds a separate test suite for explicitly testing CAN
3b6e5b9b1 Merge changes I59b6180e,Ic86e922b
b2000a20d * Fix issue if I2C initialized before DigitalSystem.
9e6d04b2e Added v10 libraries
f380d9c10 Corrected the number of CAN init attempts in C++
1ba20bc1e added space for testing gi review
a0799718f Merge "Fixes a bug in the TiltPanCameraFixture that would cause the test to freeze and never complete instead of failing"
afe1b0b34 Merge "Adds CANJaguar Java Tests to the Integration Test Suite -Makes the MotorEncoderFixture Generic for a specific motor type -Adds Methods on the FakePotentiometerSource to allow raw values to be set -Adds Runtime Printing of the Tests to indicate where we are in the program if something unexpected happens"
072b92e55 Fixes a bug in the TiltPanCameraFixture that would cause the test to freeze and never complete instead of failing
b27791544 Adds CANJaguar Java Tests to the Integration Test Suite -Makes the MotorEncoderFixture Generic for a specific motor type -Adds Methods on the FakePotentiometerSource to allow raw values to be set -Adds Runtime Printing of the Tests to indicate where we are in the program if something unexpected happens
e4e199f06 Added support for simulation time.
15e378180 Merge "Fixed C++ side of artf2604 in FRCSim - synchronized C++ codebases, updated examples."
606eaa67b Merge "CANJaguar waits in the constructor for initial status data"
6273a407e Merge "Added a C++ CANJaguar test for initial status data"
1cafebc5a CANJaguar waits in the constructor for initial status data
06f8ff922 Added a C++ CANJaguar test for initial status data
cc76500fd Merge "* Add WriteBulk and ReadOnly to C++ I2C"
ff597e6ac Fixed C++ side of artf2604 in FRCSim - synchronized C++ codebases, updated examples.
02e19a014 Merge "Added PacGoat code for C++."
c7e17b8e3 Added PacGoat code for C++.
41897af45 Merge "CANJaguar in Java now requires control mode configuration data at compile time"
73ed1d42e Merge "CANJaguar in C++ now requires control mode configuration data at compile time"
f3593c698 CANJaguar in Java now requires control mode configuration data at compile time
abed665c6 CANJaguar in C++ now requires control mode configuration data at compile time
945c7519e Merge "Fixed Java PCM test"
81a0664c5 Fixed Java PCM test
12af2db15 Merge "Java CANJaguar changes"
1c8cd1dab Merge "Adds a missing Hamcrest dependency for the Java Integration Test system"
df9b702e0 Adds a missing Hamcrest dependency for the Java Integration Test system
0d62d0985 Added C++ support for solenoids.
1bbf35081 Java CANJaguar changes
31ab66ba2 Merge "Added support for PacGoat robot for artf2591."
de9657760 Adds an error if you try to use SetVoltageRampRate in the wrong control mode
605148456 * Add WriteBulk and ReadOnly to C++ I2C
40628a817 Added support for PacGoat robot for artf2591.
0f8c83f69 Merge "Adds CommandTests to the C++ Integration Test Suite Also adds/updates some comments in the Java Command Tests"
61ca86511 Adds CommandTests to the C++ Integration Test Suite Also adds/updates some comments in the Java Command Tests
f5862582e FRCSim artf2604: Synchronized the codebases of WPILibJ-Simulation and the main WPILibJ.
698f38d40 Fixed FRCSim artf2594 - JavaGazebo no longer crashes if gzserver hasn't started, and cleaned up some code in the area.
e9ade472e Merge "Updated CAN Jaguar for C++"
21b58aebd Updated CAN Jaguar for C++
08ed0b286 Fixed eclipse bug on windows when not run from command prompt.
801efe601 Merge "Makes the ToolChain Plugins platform specific so that you can no longer install multiple versions of the tool chains simultaneously. [artf2558]"
3927967ac Merge "Added PDP tests for java and C++, updated TestBench.h"
2cc2e6a9e Added PDP tests for java and C++, updated TestBench.h
2cb9d55ef Makes the ToolChain Plugins platform specific so that you can no longer install multiple versions of the tool chains simultaneously. [artf2558]
f598abf5b Fix FRCSim artf2589 and cleanup the surrounding code.
e69c1ea19 Merge "Some much need love for the eclipse plugins."
1fb6d9fd0 Updates/Tests the AnalogPotentiometer New constructor for the AnalogPotentiometer to allow users to pass it an AnalogInput into the constructor New FakePotentiometerSource to emulate a AnalogPotentiometer
60cfabca8 Merge "Changed the AnalogPot.cpp class, added AnalogPot test for CPP"
71a66d01c Changed the AnalogPot.cpp class, added AnalogPot test for CPP
c106939bc Some much need love for the eclipse plugins.
4a5a14ad8 FRCSim installation script upgrades to fix artf2579: Improved error-handling, added environment checking, added uninstallation option.
52654851d Add C++ Counter Test
d9c1a4edf Fixes for building the Java plugin to include the JNI.
9ff741278 Fixes for rebasing.
bd1aea0d2 Adds RelayCrossConnect tests as well as AnalogCrossConnect  tests  for java Also splits up the tests into multiple TestSuites so that the they are explicitly associated with the tests in the package that they are running
fc2f0e37b Adds JNI methods for the new CANSessionMUX
974362bd8 Updates CANJNI.cpp so that it compiles properly (CAN NOT WORKING)
85dff7d2e New netcomms .so and headers.
58021f739 Removed analog and digital module numbers
aa3b24092 Adds several more tests to the PID Test Class Adds Logger output to tests when they fail to allow the rest of the tests to run while you can follow the failure stack trace. This allows you to begin working on fixing the failure before the remainder of the tests have run.
fdfc33a38 Merge "Removes the print line from the Preferences class so that the write method no longer causes a system print"
4be3bc25a Removes the print line from the Preferences class so that the write method no longer causes a system print
c7e7f39c4 Merge "Updates the PID.free() method to release the NetworkTable listener. [artf2571] Documentation update on the Depreciated method. Removes the PID.setTolerance(Tolerance tolerance) method because it was private and never used"
94894931a Updates the PID.free() method to release the NetworkTable listener. [artf2571] Documentation update on the Depreciated method. Removes the PID.setTolerance(Tolerance tolerance) method because it was private and never used
cb56c9a14 Initial commit of the WPILib simulation support in an alpha quality state.
d5a509c7e Merge "Changed AnalogChannel to AnalogInput, added AnalogOutput for MXP"
3538e2c05 Changed AnalogChannel to AnalogInput, added AnalogOutput for MXP
a4708d850 Makes the PID Test run 10 times in order to determine how many times it fails out of 10 runs per motor. (Unstable Test!)
a46c24658 Moves the Talon to port 10 to reflect the hardware change
8d5c67e6a Added Omar's changes to the compressor interface
5d646536f C++ PCM integration tests
3c5d46c2a Merge "Added support for digital and analog IO pins on the MXP"
29c4534c5 Added support for digital and analog IO pins on the MXP
d019ec982 Merge "WPILibJ More Integration Tests Refactors the MotorEncoderTest to make use of the parameterized testing framework Adds a test for the Preferences Class Updates the TiltPanCameraTest to work Makes the TimerTest use math for long values Adds a Button Test to Test the internal Button Speeds up the FakeCounterSource/FakeEncoderSource to decrease test time Moves the AbstractComsSetup Block to a static method so that it only runs once Updates the logging.properties class so that tests are quieter"
81aab6645 Adds Java debug script and updates the Ant file to run with the plugins [artf2570]
8b19137ca WPILibJ More Integration Tests Refactors the MotorEncoderTest to make use of the parameterized testing framework Adds a test for the Preferences Class Updates the TiltPanCameraTest to work Makes the TimerTest use math for long values Adds a Button Test to Test the internal Button Speeds up the FakeCounterSource/FakeEncoderSource to decrease test time Moves the AbstractComsSetup Block to a static method so that it only runs once Updates the logging.properties class so that tests are quieter
85d9ead4e Sets the default PIDSourceParameter to be kDistance so there is no null pointer exception
eb00824a5 Updates the MotorEncoder and TestBench to clarify where setup/teardown methods are called
c90c38fa2 Adds a PIDTest to the integration tests. Contributors: Jonathan Leitschuh [artf2568]
59a1fb2c0 Makes the PIDController completely thread safe and ensures that freeing the PID Loop truly clears it [artf2567]
d418271a6 Updates the HAL to use the most recent control data [artf2557]
32dc451a7 Merge "Fixes the Preferences putting the preferences in a file 'file:' and instead puts it in the root directory [artf2566]"
63ae47b7e Fixes the Preferences putting the preferences in a file 'file:' and instead puts it in the root directory [artf2566]
35ac240d4 Switch to 0-based for all pins on the RoboRIO [artf2564]
59dfb4d21 Removed the old compressor code
cf798049e Disable driver station at C++ program start
a3e11f201 More C++ tests
09feff102 Integrates tests for Commands
97fead073 Merge "Adds/Updates the JavaIntegrationTest Framework"
70ccca881 Adds/Updates the JavaIntegrationTest Framework
c937c409a Made PIDController use pthreads in C++
36470d3b7 Made Java PCMCompressor methods public
4c61ab4ea Fixed C++ PWM values to be 16 bits
15c2798de Merge "Don't do doubles"
1e3a5389b Merge "Tilt pan camera test"
3227e779a Adds .cproject to the .gitignore to remove eclipse files
2e39540f2 Tilt pan camera test
bb50f4b13 C++ testing
a3b65a684 Don't do doubles
9a145db3d Merge "Updates the C++ servo class  to have the PWM values that are on the spec sheet"
884a374f4 Updates the C++ servo class  to have the PWM values that are on the spec sheet
187aa7a13 Support for the CAN power distribution panel
bf7ef4376 Changed the default logging level to WARNING This prevents excessive logging from the HAL. Put it back as needed
5d21444c4 Fixed a missing include
b219d1915 Fixed several files to use .hpp headers
2bf9db001 Merge "Adds/Extends several integration tests. Modifies Java Servo with correct values according to the datasheet assoiciated with the Hitec HS-322HD"
efeec5d98 Adds/Extends several integration tests. Modifies Java Servo with correct values according to the datasheet assoiciated with the Hitec HS-322HD
447cec4df Support for the CAN pneumatics module in C++ and Java
980d52bec Support for the CAN pneumatics module in C++ and Java
9b831ed34 Reverting back to static .a files for C++ and fixing lots of other assorted items
c1482cb26 Made CAN actually work in Java
db940d8c4 Adds Integration tests for WPILib
859983049 Added in LiveWindow initialization code, and the network tables dependecies for WPILibJ. - Fredric Silberberg
609fbfd8f Merge "Adds the Accumulator code to the AnalogJNI.cpp Disables the debug printlines from the AnalogJNI.cpp file"
6d8629a8b Adds the Accumulator code to the AnalogJNI.cpp Disables the debug printlines from the AnalogJNI.cpp file
04b6afb1c WPILibJ Integration Tests
6b83175b7 Merge "Fixed the package for CANJaguar.java"
9925598da Merge "Updated POM files to allow use of compile phase."
9a29c684a Fixed the package for CANJaguar.java
66c9aabec Merge "Updated CANJaguar.h/.cpp/.java for the RoboRIO"
736cb754f Updated POM files to allow use of compile phase.
b955ec994 Merge "Adds maven configuration information to use m2e This also removes all of the .project and .classpath files generated by eclipse"
da2e1769c Updated CANJaguar.h/.cpp/.java for the RoboRIO
85b0a46fd Adds maven configuration information to use m2e This also removes all of the .project and .classpath files generated by eclipse
ed6ac3bff Fixed digital channel routing
7e215ef18 Removed the old cRIO channel routing that was preventing encoders from working in C++
6e01adaa5 Revert "Modifies maven files to work with m2e" This commit causes Eclipse to give a vaugue error message when creating a new Robot Java Project or Robot C++ Project.
36241b0d6 Modifies maven files to work with m2e
12e811250 Driver Station
0227264de Merge "Added .classpath to the gitignore"
059cee8b2 Merge "Fixed the module order so the libraries are built before the plugins"
5d81894d1 Merge "WPILibJ Unit Tests"
21074eea4 Added .classpath to the gitignore
acb963612 WPILibJ Unit Tests
cdc94f062 Added .gitignore file
508425c97 Fixed some typos in pom files
9f64cf615 Fixed the module order so the libraries are built before the plugins
27896984f Added the new NI shared libraries and headers
848a043f9 Fixing some compile warnings in C++
1c943ff6f Changed the build.xml files to write the runjavaprogram and runcppprogram Still need to fix the debug c++, it has references to java
82795d6a3 Fix 2 path problems in some scripts
a42a7943b Init HAL from WPILibJ to remove segfaults
e4be9eb09 Added mvn profiles to define the cmake required properties depending on the build platform.
b7ad001b4 Fixing packaging
87e9b6ce5 toolchain name...
885c3ab83 reverting building opkg in maven/cmake (now uses ant)
af2368b6f Moving opkg build to deployment
c0222d4e2 Automatic SO downloading & updating
69d9ad70a CMake Changes
33134bef1 Java 8 Compilation Fixes
4546abc8c Version 4 Image
b1ef11610 Revert "Applied patch from Dustin Spicuzza to fix robot hang"
d3ef8de5e Applied patch from Dustin Spicuzza to fix robot hang
8d1fc91e8 .gitreview file
5f727e57a Eclipse toolchain installer now preserves executable bit on Linux
260636250 Changed the CPU Architecture
aad6d11f4 Renamed log.h to Log.h
938a422b2 Added human-readable error messages to the Maven and Java version enforcer rules
1a7526fc8 Disabled NetworkTables java Azalea build by default
5d1d9fcde Added enforcer rules to restrict Maven and Java versions
616acc26d Corrected incorrect relative path to parent POM
0d5410a11 Removed all repository definitions pointing to Jenkins (aka. frcbuilder)
3924a4b6f fixed the path name for sflt
aa40fd0c5 Replaced hardcoded forward slash with system correct separator
476dccdd3 Corrected <relativePath> that pointed to directory instead of pom.xml
bbc0a6b95 Corrected <relativePath> that pointed to directory instead of pom.xml
203debb46 Updated wpilibJavaJNI JDK include path to use default location in user's home directory
2bdd95d72 Set Azalea wpilibc off by default
9880ff3fd turn off network tables desktop and azalea builds by default
3e74499d4 Turn off Azalea hal by default
f2746b7d4 Merge branch 'master' of ssh://charris@usfirst.collab.net:29418/allwpilib
28848553a missing file from original commit and push
6a87f9057 Updated <relativePath> references to static-library
155af5966 Replaced hardcoded JDK include path with property
08d06bd22 Updated parent pom <relativePath> properties to be correct
f7146d423 JNI for java Normal vs recursive mutex HAL delineation
b62b60611 JNI implementation for Java Normal vs
4297b2bc9 Merge branch 'master' of ssh://usfirst.collab.net:29418/allwpilib
d09371bc0 Initial empty repository
3178911ee Initial checkin of unified hierarchy of WPILib 2015
REVERT: 00e090877 Fix bounds of SPI AutoRead sizes
REVERT: 8fce748cf Rename our allwpilib (which is now 2020) to not have 2019 in the name

Change-Id: I8247665e5386c0696e597eafeed095e26ef12c79
git-subtree-dir: third_party/allwpilib
git-subtree-split: 66b57f0323d55b8c875f0391f48e69880a3b4389

.clang-format

diff --git a/.clang-format b/.clang-format
index 92b4049..f5a95f7 100644
--- a/.clang-format
+++ b/.clang-format
@@ -3,24 +3,29 @@
 BasedOnStyle:  Google
 AccessModifierOffset: -1
 AlignAfterOpenBracket: Align
+AlignConsecutiveMacros: false
 AlignConsecutiveAssignments: false
 AlignConsecutiveDeclarations: false
 AlignEscapedNewlines: Left
 AlignOperands:   true
 AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllConstructorInitializersOnNextLine: true
 AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: false
+AllowShortBlocksOnASingleLine: Never
 AllowShortCaseLabelsOnASingleLine: false
 AllowShortFunctionsOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: true
+AllowShortLambdasOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: WithoutElse
 AllowShortLoopsOnASingleLine: true
 AlwaysBreakAfterDefinitionReturnType: None
 AlwaysBreakAfterReturnType: None
 AlwaysBreakBeforeMultilineStrings: true
-AlwaysBreakTemplateDeclarations: true
+AlwaysBreakTemplateDeclarations: Yes
 BinPackArguments: true
 BinPackParameters: true
 BraceWrapping:
+  AfterCaseLabel:  false
   AfterClass:      false
   AfterControlStatement: false
   AfterEnum:       false
@@ -29,6 +34,7 @@
   AfterObjCDeclaration: false
   AfterStruct:     false
   AfterUnion:      false
+  AfterExternBlock: false
   BeforeCatch:     false
   BeforeElse:      false
   IndentBraces:    false
@@ -38,6 +44,7 @@
 BreakBeforeBinaryOperators: None
 BreakBeforeBraces: Attach
 BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
 BreakBeforeTernaryOperators: true
 BreakConstructorInitializersBeforeComma: false
 BreakConstructorInitializers: BeforeColon
@@ -50,6 +57,7 @@
 ConstructorInitializerIndentWidth: 4
 ContinuationIndentWidth: 4
 Cpp11BracedListStyle: true
+DeriveLineEnding: true
 DerivePointerAlignment: false
 DisableFormat:   false
 ExperimentalAutoDetectBinPacking: false
@@ -58,15 +66,25 @@
   - foreach
   - Q_FOREACH
   - BOOST_FOREACH
+IncludeBlocks:   Regroup
 IncludeCategories:
+  - Regex:           '^<ext/.*\.h>'
+    Priority:        2
+    SortPriority:    0
   - Regex:           '^<.*\.h>'
     Priority:        1
+    SortPriority:    0
   - Regex:           '^<.*'
     Priority:        2
+    SortPriority:    0
   - Regex:           '.*'
     Priority:        3
+    SortPriority:    0
 IncludeIsMainRegex: '([-_](test|unittest))?$'
+IncludeIsMainSourceRegex: ''
 IndentCaseLabels: true
+IndentGotoLabels: true
+IndentPPDirectives: None
 IndentWidth:     2
 IndentWrappedFunctionNames: false
 JavaScriptQuotes: Leave
@@ -76,32 +94,73 @@
 MacroBlockEnd:   ''
 MaxEmptyLinesToKeep: 1
 NamespaceIndentation: None
+ObjCBinPackProtocolList: Never
 ObjCBlockIndentWidth: 2
 ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: false
+ObjCSpaceBeforeProtocolList: true
 PenaltyBreakAssignment: 2
 PenaltyBreakBeforeFirstCallParameter: 1
 PenaltyBreakComment: 300
 PenaltyBreakFirstLessLess: 120
 PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
 PenaltyExcessCharacter: 1000000
 PenaltyReturnTypeOnItsOwnLine: 200
 PointerAlignment: Left
+RawStringFormats:
+  - Language:        Cpp
+    Delimiters:
+      - cc
+      - CC
+      - cpp
+      - Cpp
+      - CPP
+      - 'c++'
+      - 'C++'
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
+  - Language:        TextProto
+    Delimiters:
+      - pb
+      - PB
+      - proto
+      - PROTO
+    EnclosingFunctions:
+      - EqualsProto
+      - EquivToProto
+      - PARSE_PARTIAL_TEXT_PROTO
+      - PARSE_TEST_PROTO
+      - PARSE_TEXT_PROTO
+      - ParseTextOrDie
+      - ParseTextProtoOrDie
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
 ReflowComments:  true
 SortIncludes:    false
 SortUsingDeclarations: true
 SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
 SpaceAfterTemplateKeyword: true
 SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
 SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: false
 SpaceInEmptyParentheses: false
 SpacesBeforeTrailingComments: 2
 SpacesInAngles:  false
+SpacesInConditionalStatement: false
 SpacesInContainerLiterals: true
 SpacesInCStyleCastParentheses: false
 SpacesInParentheses: false
 SpacesInSquareBrackets: false
+SpaceBeforeSquareBrackets: false
 Standard:        Auto
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
 TabWidth:        8
 UseTab:          Never
 ...

.github/CODEOWNERS

diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
new file mode 100644
index 0000000..3b6f7a1
--- /dev/null
+++ b/.github/CODEOWNERS
@@ -0,0 +1,38 @@
+# Lines starting with '#' are comments.
+# Each line is a file pattern followed by one or more owners.
+
+# More details are here: https://help.github.com/articles/about-codeowners/
+
+# The '*' pattern is global owners.
+
+# Order is important. The last matching pattern has the most precedence.
+
+# The folders are ordered as a tree: by depth then alphabetically.
+# This should make it easy to add new rules without breaking existing ones.
+
+# Global rule:
+*  @wpilibsuite/wpilib
+
+/cameraserver/  @wpilibsuite/camera-server
+
+/cscore/  @wpilibsuite/camera-server
+
+/hal/  @wpilibsuite/hardware
+/hal/src/main/java/**/sim/  @wpilibsuite/simulation
+/hal/src/main/native/include/mockdata/  @wpilibsuite/simulation
+/hal/src/main/native/include/simulation/  @wpilibsuite/simulation
+/hal/src/main/native/sim/  @wpilibsuite/simulation
+
+/ntcore/  @wpilibsuite/network-tables
+
+/simulation/  @wpilibsuite/simulation
+
+/wpilibNewCommands/  @wpilibsuite/commandbased
+
+/wpilibOldCommands/  @wpilibsuite/commandbased
+
+/wpilibcExamples/  @wpilibsuite/wpilib @wpilibsuite/documentation
+
+/wpilibjExamples/  @wpilibsuite/wpilib @wpilibsuite/documentation
+
+/wpiutil/  @PeterJohnson

.github/ISSUE_TEMPLATE/bug_report.md

diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md
new file mode 100644
index 0000000..76b7301
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/bug_report.md
@@ -0,0 +1,30 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: ''
+labels: bug
+assignees: ''
+
+---
+
+**Describe the bug**
+A clear and concise description of what the bug is.
+
+**To Reproduce**
+Steps to reproduce the behavior:
+1. ...
+2. ...
+
+**Expected behavior**
+A clear and concise description of what you expected to happen.
+
+**Screenshots**
+If applicable, add screenshots to help explain your problem.
+
+**Desktop (please complete the following information):**
+ - OS: [e.g. Windows]
+ - Java version [e.g. 1.10.2]
+ - C++ version [e.g. 17]
+
+**Additional context**
+Add any other context about the problem here.

.github/ISSUE_TEMPLATE/feature_request.md

diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md
new file mode 100644
index 0000000..11fc491
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/feature_request.md
@@ -0,0 +1,20 @@
+---
+name: Feature request
+about: Suggest an idea for this project
+title: ''
+labels: enhancement
+assignees: ''
+
+---
+
+**Is your feature request related to a problem? Please describe.**
+A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+
+**Describe the solution you'd like**
+A clear and concise description of what you want to happen.
+
+**Describe alternatives you've considered**
+A clear and concise description of any alternative solutions or features you've considered.
+
+**Additional context**
+Add any other context or screenshots about the feature request here.

.github/ISSUE_TEMPLATE/question.md

diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md
new file mode 100644
index 0000000..61420e3
--- /dev/null
+++ b/.github/ISSUE_TEMPLATE/question.md
@@ -0,0 +1,20 @@
+---
+name: Question
+about: Ask about features or parts of this project
+title: ''
+labels: ''
+assignees: ''
+
+---
+
+**Describe the question you have.**
+A clear and concise description of what you want clarification on.
+
+**Describe the reason for your confusion.**
+A clear and concise description of why the question requires clarification and what's confusing.
+
+**Is your question related to a problem? Please describe.**
+A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+
+**Additional context**
+Add any other context or screenshots about the question here.

.github/workflows/ci.yml

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
new file mode 100644
index 0000000..3a8022b
--- /dev/null
+++ b/.github/workflows/ci.yml
@@ -0,0 +1,238 @@
+name: CI
+
+on: [pull_request, push]
+
+jobs:
+  build-docker:
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - container: wpilib/roborio-cross-ubuntu:2021-18.04
+            artifact-name: Athena
+            build-options: "-Ponlylinuxathena"
+          - container: wpilib/raspbian-cross-ubuntu:10-18.04
+            artifact-name: Raspbian
+            build-options: "-Ponlylinuxraspbian"
+          - container: wpilib/aarch64-cross-ubuntu:bionic-18.04
+            artifact-name: Aarch64
+            build-options: "-Ponlylinuxaarch64bionic"
+          - container: wpilib/ubuntu-base:18.04
+            artifact-name: Linux
+            build-options: "-Dorg.gradle.jvmargs=-Xmx2g"
+    name: "Build - ${{ matrix.artifact-name }}"
+    runs-on: ubuntu-latest
+    container: ${{ matrix.container }}
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v')
+      - name: Build with Gradle
+        run: ./gradlew build -PbuildServer ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+      - uses: actions/upload-artifact@v2
+        with:
+          name: ${{ matrix.artifact-name }}
+          path: build/allOutputs
+
+  build-host:
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - os: windows-latest
+            artifact-name: Win64
+            architecture: x64
+          - os: windows-latest
+            artifact-name: Win32
+            architecture: x86
+          - os: macos-latest
+            artifact-name: macOS
+            architecture: x64
+    name: "Build - ${{ matrix.artifact-name }}"
+    runs-on: ${{ matrix.os }}
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 11
+          architecture: ${{ matrix.architecture }}
+      - name: Import Developer ID Certificate
+        uses: wpilibsuite/import-signing-certificate@v1
+        with:
+          certificate-data: ${{ secrets.APPLE_CERTIFICATE_DATA }}
+          certificate-passphrase: ${{ secrets.APPLE_CERTIFICATE_PASSWORD }}
+          keychain-password: ${{ secrets.APPLE_KEYCHAIN_PASSWORD }}
+        if: |
+          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
+      - name: Set Keychain Lock Timeout
+        run: security set-keychain-settings -lut 3600
+        if: |
+          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        shell: bash
+        if: startsWith(github.ref, 'refs/tags/v')
+      - name: Build with Gradle
+        run: ./gradlew build -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
+      - name: Sign Libraries with Developer ID
+        run: ./gradlew build -PbuildServer -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ env.EXTRA_GRADLE_ARGS }}
+        if: |
+          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
+      - uses: actions/upload-artifact@v2
+        with:
+          name: ${{ matrix.artifact-name }}
+          path: build/allOutputs
+
+  build-documentation:
+    name: "Build - Documentation"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 13
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v')
+      - name: Build with Gradle
+        run: ./gradlew docs:zipDocs -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
+      - uses: actions/upload-artifact@v2
+        with:
+          name: Documentation
+          path: docs/build/outputs
+
+  build-cmake:
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - os: ubuntu-latest
+            name: Linux
+            container: wpilib/roborio-cross-ubuntu:2020-18.04
+            flags: ""
+          - os: macos-latest
+            name: macOS
+            container: ""
+            flags: "-DWITH_JAVA=OFF"
+    name: "Build - CMake ${{ matrix.name }}"
+    runs-on: ${{ matrix.os }}
+    container: ${{ matrix.container }}
+    steps:
+      - uses: actions/checkout@v2
+      - name: Install Dependencies
+        run: |
+          if [ "$RUNNER_OS" == "macOS" ]; then
+            brew install opencv
+          fi
+      - name: configure
+        run: mkdir build && cd build && cmake -DWITH_GUI=OFF ${{ matrix.flags }} ..
+      - name: build
+        working-directory: build
+        run: make -j3
+      - name: test
+        working-directory: build
+        run: make test
+
+  build-cmake-vcpkg:
+    name: "Build - CMake Windows"
+    runs-on: windows-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Prepare vcpkg
+        uses: lukka/run-vcpkg@v4
+        with:
+          vcpkgArguments: opencv
+          vcpkgDirectory: ${{ runner.workspace }}/vcpkg/
+          vcpkgGitCommitId: 544f8e4593764f78faa94bac2adb81cca5232943
+          vcpkgTriplet: x64-windows
+      - name: Configure & Build
+        uses: lukka/run-cmake@v3
+        with:
+          buildDirectory: ${{ runner.workspace }}/build/
+          cmakeAppendedArgs: -DWITH_JAVA=OFF -DWITH_GUI=OFF
+          cmakeListsOrSettingsJson: CMakeListsTxtAdvanced
+          useVcpkgToolchainFile: true
+      - name: Run Tests
+        run: ctest -C "Debug"
+        working-directory: ${{ runner.workspace }}/build/
+
+  wpiformat:
+    name: "wpiformat"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Fetch all history and metadata
+        run: |
+          git fetch --prune --unshallow
+          git checkout -b pr
+          git branch -f master origin/master
+      - name: Set up Python 3.8
+        uses: actions/setup-python@v2
+        with:
+          python-version: 3.8
+      - name: Install clang-format
+        run: sudo apt-get update -q && sudo apt-get install clang-format-10
+      - name: Install wpiformat
+        run: pip3 install wpiformat
+      - name: Run
+        run: wpiformat -clang 10
+      - name: Check Output
+        run: git --no-pager diff --exit-code HEAD
+
+  combine:
+    name: Combine
+    needs: [build-docker, build-host, build-documentation]
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          repository: wpilibsuite/build-tools
+      - uses: actions/download-artifact@v2
+        with:
+          path: combiner/products/build/allOutputs
+      - name: Flatten Artifacts
+        run: rsync -a --delete combiner/products/build/allOutputs/*/* combiner/products/build/allOutputs/
+      - name: Check version number exists
+        run: |
+          cat combiner/products/build/allOutputs/version.txt
+          test -s combiner/products/build/allOutputs/version.txt
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 11
+      - name: Combine
+        if: |
+          !startsWith(github.ref, 'refs/tags/v') &&
+          github.ref != 'refs/heads/master'
+        run: cd combiner && ./gradlew publish -Pallwpilib
+      - name: Combine (Master)
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          github.ref == 'refs/heads/master'
+        run: cd combiner && ./gradlew publish -Pallwpilib
+        env:
+          RUN_AZURE_ARTIFACTORY_RELEASE: 'TRUE'
+          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
+          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
+      - name: Combine (Release)
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          startsWith(github.ref, 'refs/tags/v')
+        run: cd combiner && ./gradlew publish -Pallwpilib -PreleaseRepoPublish
+        env:
+          RUN_AZURE_ARTIFACTORY_RELEASE: 'TRUE'
+          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
+          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
+      - uses: actions/upload-artifact@v2
+        with:
+          name: Maven
+          path: ~/releases

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

diff --git a/.github/workflows/gradle-wrapper-validation.yml b/.github/workflows/gradle-wrapper-validation.yml
new file mode 100644
index 0000000..405a2b3
--- /dev/null
+++ b/.github/workflows/gradle-wrapper-validation.yml
@@ -0,0 +1,10 @@
+name: "Validate Gradle Wrapper"
+on: [push, pull_request]
+
+jobs:
+  validation:
+    name: "Validation"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - uses: gradle/wrapper-validation-action@v1

.gitignore

diff --git a/.gitignore b/.gitignore
index bdf94fe..524d81d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -222,3 +222,5 @@
 # clang configuration and clangd cache
 .clang
 .clangd/
+
+imgui.ini

.styleguide

diff --git a/.styleguide b/.styleguide
index d465d74..485ac26 100644
--- a/.styleguide
+++ b/.styleguide
@@ -2,6 +2,7 @@
   \.h$
   \.hpp$
   \.inc$
+  \.inl$
 }
 
 cppSrcFileInclude {
@@ -18,16 +19,22 @@
 }
 
 includeOtherLibs {
+  ^Eigen/
   ^cameraserver/
   ^cscore
+  ^drake/
   ^hal/
   ^imgui
+  ^implot
   ^mockdata/
   ^networktables/
   ^ntcore
   ^opencv2/
   ^support/
   ^units/
+  ^unsupported/
   ^vision/
   ^wpi/
+  ^wpigui
+  ^wpimath/
 }

BUILD

diff --git a/BUILD b/BUILD
deleted file mode 100644
index 74612e5..0000000
--- a/BUILD
+++ /dev/null
@@ -1,90 +0,0 @@
-licenses(["notice"])
-
-# Names of WPILib "devices" I don't want to deal with making trivial updates to
-# chop out various ugliness or have to vet for sanity.
-_excluded_devices = [
-    "AnalogGyro",
-    "SerialPort",
-    "SerialHelper",
-    "visa",
-]
-
-# Header files we don't want to have.
-_bad_hdrs = ([
-    "hal/include/HAL/LabVIEW/HAL.h",
-] + ["**/%s.*" % d for d in _excluded_devices])
-
-_hal_header_dirs = [
-    "hal/src/main/native/athena",
-    "hal/src/main/native/include",
-]
-
-_hal_h_hdrs = glob(
-    [d + "/**/*.h" for d in _hal_header_dirs],
-    exclude = _bad_hdrs,
-)
-
-_hal_hpp_hdrs = glob(
-    [d + "/**/*.hpp" for d in _hal_header_dirs],
-    exclude = _bad_hdrs,
-)
-
-py_binary(
-    name = "generate_FRCUsageReporting",
-    srcs = [
-        "generate_FRCUsageReporting.py",
-    ],
-)
-
-genrule(
-    name = "do_generate_FRCUsageReporting",
-    srcs = [
-        "hal/src/generate/FRCUsageReporting.h.in",
-        "hal/src/generate/Instances.txt",
-        "hal/src/generate/ResourceType.txt",
-    ],
-    outs = [
-        "hal/src/main/native/include/hal/FRCUsageReporting.h",
-    ],
-    cmd = " ".join([
-        "$(location :generate_FRCUsageReporting)",
-        "$(location hal/src/generate/FRCUsageReporting.h.in)",
-        "$(location hal/src/generate/Instances.txt)",
-        "$(location hal/src/generate/ResourceType.txt)",
-        "$(location hal/src/main/native/include/hal/FRCUsageReporting.h)",
-    ]),
-    tools = [
-        ":generate_FRCUsageReporting",
-    ],
-)
-
-cc_library(
-    name = "hal",
-    srcs = glob(
-        include = [
-            "hal/src/main/native/athena/*.cpp",
-            "hal/src/main/native/cpp/cpp/*.cpp",
-            "hal/src/main/native/cpp/*.cpp",
-            "hal/src/main/native/athena/ctre/*.cpp",
-            "hal/src/main/native/shared/handles/*.cpp",
-            "hal/src/main/native/cpp/handles/*.cpp",
-        ],
-        exclude = ["**/%s.*" % d for d in _excluded_devices],
-    ),
-    hdrs = _hal_h_hdrs + _hal_hpp_hdrs + [
-        "hal/src/main/native/include/hal/FRCUsageReporting.h",
-    ],
-    copts = [
-        "-Wno-unused-parameter",
-        "-Wno-cast-align",
-    ],
-    defines = ["WPILIB2019=1"],
-    includes = _hal_header_dirs,
-    restricted_to = ["//tools:roborio"],
-    visibility = ["//third_party:__pkg__"],
-    deps = [
-        "//aos/logging",
-        "//third_party/allwpilib/wpiutil",
-        "@allwpilib_ni_libraries//:ni-libraries",
-    ],
-)

CMakeLists.txt

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7a54492..399421f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,10 +16,6 @@
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
 set_property(GLOBAL PROPERTY USE_FOLDERS ON)
 
-if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT AND MSVC)
-    set(CMAKE_INSTALL_PREFIX "${CMAKE_BINARY_DIR}/install" CACHE PATH "Default install dir on windows" FORCE)
-endif()
-
 set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
 set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
@@ -44,23 +40,69 @@
    SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/wpilib/lib")
 ENDIF("${isSystemDir}" STREQUAL "-1")
 
-option(WITHOUT_JAVA "don't include java and JNI in the build" OFF)
-option(BUILD_SHARED_LIBS "build with shared libs (needed for JNI)" ON)
-option(WITHOUT_CSCORE "Don't build cscore (removes OpenCV requirement)" OFF)
-option(WITHOUT_ALLWPILIB "Don't build allwpilib (removes OpenCV requirement)" ON)
-option(WITH_TESTS "build unit tests (requires internet connection)" OFF)
-option(USE_EXTERNAL_HAL "Use a separately built HAL" OFF)
+# Options for building certain parts of the repo. Everything is built by default.
+option(BUILD_SHARED_LIBS "Build with shared libs (needed for JNI)" ON)
+option(WITH_JAVA "Include java and JNI in the build" ON)
+option(WITH_CSCORE "Build cscore (needs OpenCV)" ON)
+option(WITH_WPILIB "Build hal, wpilibc/j, wpimath, and myRobot (needs OpenCV)" ON)
+option(WITH_TESTS "Build unit tests (requires internet connection)" ON)
+option(WITH_GUI "Build GUI items" ON)
+option(WITH_SIMULATION_MODULES "Build simulation modules" ON)
+
+# Options for external HAL.
+option(WITH_EXTERNAL_HAL "Use a separately built HAL" OFF)
 set(EXTERNAL_HAL_FILE "" CACHE FILEPATH "Location to look for an external HAL CMake File")
+
+# Options for using a package manager (vcpkg) for certain dependencies.
 option(USE_VCPKG_LIBUV "Use vcpkg libuv" OFF)
 option(USE_VCPKG_EIGEN "Use vcpkg eigen" OFF)
-option(FLAT_INSTALL_WPILIB "Use a flat install directory" OFF)
-option(WITH_SIMULATION_MODULES "build simulation modules" OFF)
 
-if (NOT WITHOUT_JAVA AND NOT BUILD_SHARED_LIBS)
+# Options for installation.
+option(WITH_FLAT_INSTALL "Use a flat install directory" OFF)
+
+# Options for location of OpenCV Java.
+set(OPENCV_JAVA_INSTALL_DIR "" CACHE PATH "Location to search for the OpenCV jar file")
+
+# Set default build type to release with debug info (i.e. release mode optimizations
+# are performed, but debug info still exists).
+if (NOT CMAKE_BUILD_TYPE)
+    set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "" FORCE)
+endif()
+
+# We always want flat install with MSVC.
+if (MSVC)
+  set(WITH_FLAT_INSTALL ON)
+endif()
+
+if (WITH_JAVA AND NOT BUILD_SHARED_LIBS)
     message(FATAL_ERROR "
 FATAL: Cannot build static libs with Java enabled.
        Static libs requires both BUILD_SHARED_LIBS=OFF and
-       WITHOUT_JAVA=ON
+       WITH_JAVA=OFF
+")
+endif()
+
+if (WITH_SIMULATION_MODULES AND NOT BUILD_SHARED_LIBS)
+    message(FATAL_ERROR "
+FATAL: Cannot build static libs with simulation modules enabled.
+       Static libs requires both BUILD_SHARED_LIBS=OFF and
+       WITH_SIMULATION_MODULES=OFF
+")
+endif()
+
+if (NOT WITH_JAVA OR NOT WITH_CSCORE)
+    if(NOT "${OPENCV_JAVA_INSTALL_DIR}" STREQUAL "")
+        message(WARNING "
+WARNING: OpenCV Java dir set but java is not enabled!
+It will be ignored.
+")
+    endif()
+endif()
+
+if (NOT WITH_WPILIB AND WITH_SIMULATION_MODULES)
+    message(FATAL_ERROR "
+FATAL: Cannot build simulation modules with wpilib disabled.
+       Enable wpilib by setting WITH_WPILIB=ON
 ")
 endif()
 
@@ -70,7 +112,7 @@
 set( java_lib_dest wpilib/java )
 set( jni_lib_dest wpilib/jni )
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
     set (wpilib_config_dir ${wpilib_dest})
 else()
     set (wpilib_config_dir share/wpilib)
@@ -84,12 +126,13 @@
 set (EIGEN_VCPKG_REPLACE "find_package(Eigen3 CONFIG)")
 endif()
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
 set(WPIUTIL_DEP_REPLACE "include($\{SELF_DIR\}/wpiutil-config.cmake)")
 set(NTCORE_DEP_REPLACE "include($\{SELF_DIR\}/ntcore-config.cmake)")
 set(CSCORE_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cscore-config.cmake)")
 set(CAMERASERVER_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cameraserver-config.cmake)")
 set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)")
+set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)")
 set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)")
 else()
 set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
@@ -97,6 +140,7 @@
 set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)")
 set(CAMERASERVER_DEP_REPLACE_IMPL "find_dependency(cameraserver)")
 set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)")
+set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
 set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)")
 endif()
 
@@ -112,23 +156,28 @@
 add_subdirectory(wpiutil)
 add_subdirectory(ntcore)
 
-if (NOT WITHOUT_CSCORE)
+if (WITH_GUI)
+    add_subdirectory(imgui)
+    add_subdirectory(wpigui)
+endif()
+
+if (WITH_CSCORE)
     set(CSCORE_DEP_REPLACE ${CSCORE_DEP_REPLACE_IMPL})
     set(CAMERASERVER_DEP_REPLACE ${CAMERASERVER_DEP_REPLACE_IMPL})
     add_subdirectory(cscore)
     add_subdirectory(cameraserver)
-    if (NOT WITHOUT_ALLWPILIB)
+    if (WITH_WPILIB)
         set(HAL_DEP_REPLACE ${HAL_DEP_REPLACE_IMPL})
         set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
         add_subdirectory(hal)
+        add_subdirectory(wpimath)
         add_subdirectory(wpilibj)
         add_subdirectory(wpilibc)
         add_subdirectory(myRobot)
     endif()
 endif()
 
-if (WITH_SIMULATION_MODULES AND NOT USE_EXTERNAL_HAL)
-    add_subdirectory(imgui)
+if (WITH_SIMULATION_MODULES AND NOT WITH_EXTERNAL_HAL)
     add_subdirectory(simulation)
 endif()
 

CODE_OF_CONDUCT.md

diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md
new file mode 100644
index 0000000..3d3b7e8
--- /dev/null
+++ b/CODE_OF_CONDUCT.md
@@ -0,0 +1,131 @@
+# Contributor Covenant Code of Conduct
+
+## Our Pledge
+
+We as members, contributors, and leaders pledge to make participation in our
+community a harassment-free experience for everyone, regardless of age, body
+size, visible or invisible disability, ethnicity, sex characteristics, gender
+identity and expression, level of experience, education, socio-economic status,
+nationality, personal appearance, race, religion, or sexual identity
+and orientation.
+
+We pledge to act and interact in ways that contribute to an open, welcoming,
+diverse, inclusive, and healthy community.
+
+## Our Standards
+
+Examples of behavior that contributes to a positive environment for our
+community include:
+
+* Exhibiting Gracious Professionalism® at all times. Gracious Professionalism
+  is a way of doing things that encourages high-quality work, emphasizes the
+  value of others, and respects individuals and the community.
+* Demonstrating empathy and kindness toward other people
+* Being respectful of differing opinions, viewpoints, and experiences
+* Giving and gracefully accepting constructive feedback
+* Accepting responsibility and apologizing to those affected by our mistakes,
+  and learning from the experience
+* Focusing on what is best not just for us as individuals, but for the
+  overall community
+
+Examples of unacceptable behavior include:
+
+* The use of sexualized language or imagery, and sexual attention or
+  advances of any kind
+* Trolling, insulting or derogatory comments, and personal or political attacks
+* Public or private harassment
+* Publishing others' private information, such as a physical or email
+  address, without their explicit permission
+* Other conduct which could reasonably be considered inappropriate in a
+  professional setting
+
+## Enforcement Responsibilities
+
+Community leaders are responsible for clarifying and enforcing our standards of
+acceptable behavior and will take appropriate and fair corrective action in
+response to any behavior that they deem inappropriate, threatening, offensive,
+or harmful.
+
+Community leaders have the right and responsibility to remove, edit, or reject
+comments, commits, code, wiki edits, issues, and other contributions that are
+not aligned to this Code of Conduct, and will communicate reasons for moderation
+decisions when appropriate.
+
+## Scope
+
+This Code of Conduct applies within all community spaces, and also applies when
+an individual is officially representing the community in public spaces.
+Examples of representing our community include using an official e-mail address,
+posting via an official social media account, or acting as an appointed
+representative at an online or offline event.
+
+## Enforcement
+
+Instances of abusive, harassing, or otherwise unacceptable behavior may be
+reported to the community leaders responsible for enforcement at
+[conduct@wpilib.org](mailto:conduct@wpilib.org).
+All complaints will be reviewed and investigated promptly and fairly.
+
+All community leaders are obligated to respect the privacy and security of the
+reporter of any incident.
+
+## Enforcement Guidelines
+
+Community leaders will follow these Community Impact Guidelines in determining
+the consequences for any action they deem in violation of this Code of Conduct:
+
+### 1. Correction
+
+**Community Impact**: Use of inappropriate language or other behavior deemed
+unprofessional or unwelcome in the community.
+
+**Consequence**: A private, written warning from community leaders, providing
+clarity around the nature of the violation and an explanation of why the
+behavior was inappropriate. A public apology may be requested.
+
+### 2. Warning
+
+**Community Impact**: A violation through a single incident or series
+of actions.
+
+**Consequence**: A warning with consequences for continued behavior. No
+interaction with the people involved, including unsolicited interaction with
+those enforcing the Code of Conduct, for a specified period of time. This
+includes avoiding interactions in community spaces as well as external channels
+like social media. Violating these terms may lead to a temporary or
+permanent ban.
+
+### 3. Temporary Ban
+
+**Community Impact**: A serious violation of community standards, including
+sustained inappropriate behavior.
+
+**Consequence**: A temporary ban from any sort of interaction or public
+communication with the community for a specified period of time. No public or
+private interaction with the people involved, including unsolicited interaction
+with those enforcing the Code of Conduct, is allowed during this period.
+Violating these terms may lead to a permanent ban.
+
+### 4. Permanent Ban
+
+**Community Impact**: Demonstrating a pattern of violation of community
+standards, including sustained inappropriate behavior, harassment of an
+individual, or aggression toward or disparagement of classes of individuals.
+
+**Consequence**: A permanent ban from any sort of public interaction within
+the community.
+
+## Attribution
+
+This Code of Conduct is adapted from the [Contributor Covenant][homepage],
+version 2.0, available at
+https://www.contributor-covenant.org/version/2/0/code_of_conduct.html.
+
+Community Impact Guidelines were inspired by [Mozilla's code of conduct
+enforcement ladder](https://github.com/mozilla/diversity).
+
+[homepage]: https://www.contributor-covenant.org
+
+For answers to common questions about this code of conduct, see the FAQ at
+https://www.contributor-covenant.org/faq. Translations are available at
+https://www.contributor-covenant.org/translations.

CONTRIBUTING.md

diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 509f2e2..8409c5e 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -1,6 +1,6 @@
 # Contributing to WPILib
 
-So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules written here, and behave with Gracious Professionalism.
+So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules in the [code of conduct](https://github.com/wpilibsuite/allwpilib/blob/master/CODE_OF_CONDUCT.md), and behave with Gracious Professionalism.
 
 - [General Contribution Rules](#general-contribution-rules)
 - [What to Contribute](#what-to-contribute)
@@ -37,7 +37,7 @@
 
 ## Coding Guidelines
 
-WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 6.0 with wpiformat.
+WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 10.0 with wpiformat.
 
 While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
 
@@ -45,12 +45,12 @@
 
 ### Pull Request Format
 
-Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, we ask that you squash your changes down to a single commit. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. No change will be merged unless it is up to date with the current master. We will also not merge any changes with merge commits in them; please rebase off of master before submitting a pull request. We do this to make sure that the git history isn't too cluttered.
+Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current master. We do this to make sure that the git history isn't too cluttered.
 
 ### Merge Process
 
-When you first submit changes, Travis-CI will attempt to run `./gradlew check` on your change. If this fails, you will need to fix any issues that it sees. Once Travis passes, we will begin the review process in more earnest. One or more WPILib team members will review your change. This will be a back-and-forth process with the WPILib team and the greater community. Once we are satisfied that your change is ready, we will allow our Jenkins instance to test it. This will run the full gamut of checks, including integration tests on actual hardware. Once all tests have passed and the team is satisfied, we will merge your change into the WPILib repository.
+When you first submit changes, GitHub Actions will attempt to run `./gradlew check` on your change. If this fails, you will need to fix any issues that it sees. Once Actions passes, we will begin the review process in more earnest. One or more WPILib team members will review your change. This will be a back-and-forth process with the WPILib team and the greater community. Once we are satisfied that your change is ready, we will allow our hosted instance to test it. This will run the full gamut of checks, including integration tests on actual hardware. Once all tests have passed and the team is satisfied, we will merge your change into the WPILib repository.
 
 ## Licensing
 
-By contributing to WPILib, you agree that your code will be distributed with WPILib, and licensed under the license for the WPILib project. You should not contribute code that you do not have permission to relicense in this manner. This includes code that is licensed under the GPL that you do not have permission to relicense, as WPILib is not released under a copyleft license. Our license is the 3-clause BSD license, which you can find [here](LICENSE.txt).
+By contributing to WPILib, you agree that your code will be distributed with WPILib, and licensed under the license for the WPILib project. You should not contribute code that you do not have permission to relicense in this manner. This includes code that is licensed under the GPL that you do not have permission to relicense, as WPILib is not released under a copyleft license. Our license is the 3-clause BSD license, which you can find [here](LICENSE.md).

FasterBuilds.md

diff --git a/FasterBuilds.md b/FasterBuilds.md
deleted file mode 100644
index 8963442..0000000
--- a/FasterBuilds.md
+++ /dev/null
@@ -1,15 +0,0 @@
-# Faster Builds for Developers
-
-When you run `./gradlew build`, it builds EVERYTHING. This means debug and release builds for desktop and all installed cross compilers. For many developers, this is way too much, and causes much developer pain.
-
-To help with some of these things, common tasks have shortcuts to only build necessary things for common development and testing tasks.
-
-## Development (Desktop)
-
-For projects `wpiutil`, `ntcore`, `cscore`, `hal` `wpilibOldCommands`, `wpilibNewCommands` and `cameraserver`, a `testDesktopJava` and a `testDesktopCpp` task exists. These can be ran with `./gradlew :projectName:task`, and will only build the minimum things required to run those tests.
-
-For `wpilibc`, a `testDesktopCpp` task exists. For `wpilibj`, a `testDesktopJava` task exists.
-
-For `wpilibcExamples`, a `buildDesktopCpp` task exists (These can't be ran, but they can compile).
-
-For `wpilibjExamples`, a `buildDesktopJava` task exists.

LICENSE.md

diff --git a/LICENSE.md b/LICENSE.md
new file mode 100644
index 0000000..a8cd035
--- /dev/null
+++ b/LICENSE.md
@@ -0,0 +1,24 @@
+Copyright (c) 2009-2019 FIRST
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of the FIRST nor the
+     names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

LICENSE.txt

diff --git a/LICENSE.txt b/LICENSE.txt
deleted file mode 100644
index 1b05ea8..0000000
--- a/LICENSE.txt
+++ /dev/null
@@ -1,24 +0,0 @@
-Copyright (c) 2009-2018 FIRST
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the FIRST nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
-EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
-PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
-ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

MAINTAINERS.md

diff --git a/MAINTAINERS.md b/MAINTAINERS.md
new file mode 100644
index 0000000..63a9a57
--- /dev/null
+++ b/MAINTAINERS.md
@@ -0,0 +1,27 @@
+## Publishing Third Party Dependencies

+Currently the 3rd party deps are imgui, opencv, and google test

+

+For publishing these dependencies, the version needs to be manually updated in the publish.gradle file of their respective repository.

+Then, in the azure build for the dependency you want to build for, manually start a pipeline build (As of current, this is the `Run Pipeline` button).

+A variable needs to be added called `RUN_AZURE_ARTIFACTORY_RELEASE`, with a value of `true`. Then when the pipeline gets started, the final build outputs will be updated to artifactory.

+

+To use newer versions of C++ dependencies, in `shared/config.gradle`, update the version related to the specific dependency.

+For Java dependencies, there is likely a file related to the specific dependency in the shared folder. Update the version in there.

+

+Note, changing artifact locations (This includes changing the artifact year currently, I have an issue open to change this) requires updating the `native-utils` plugin

+

+## Publishing allwpilib

+allwpilib publishes to the development repo on every push to master. To publish a release build, upload a new tag, and a release will automatically be built and published.

+

+## Publishing desktop tools

+Desktop tools publish to the development repo on every push to master. To publish a release build, upload a new tag, and a release will automatically be built and published.

+

+## Publishing VS Code

+Before publishing, make sure to update the gradlerio version in `vscode-wpilib/resources/gradle/version.txt` Also make sure the gradle wrapper version matches the wrapper required by gradlerio.

+Upon pushing a tag, a release will be built, and the files will be uploaded to the releases on GitHub. For publishing to the marketplace, you need a Microsoft account and to be added as a maintainer.

+

+## Publishing GradleRIO

+Before publishing, make sure to update the version in build.gradle. Publishing must happen locally, using the command `./gradlew publishPlugin`. This does require your API key for publishing to be set.

+

+## Building the installer

+Update the GradleRIO version in gradle.properties, and in the scripts folder in vscode, update the vscode extension. Then push, it will build the installer on azure.

MavenArtifacts.md

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

OtherVersions.md

diff --git a/OtherVersions.md b/OtherVersions.md
new file mode 100644
index 0000000..a335f55
--- /dev/null
+++ b/OtherVersions.md
@@ -0,0 +1,68 @@
+# Installing Development Builds
+
+This article contains instructions on building projects using a development build and a local WPILib build.
+
+**Note:** This only applies to Java/C++ teams.
+
+## Development Build
+
+Development builds are the per-commit build hosted everytime a commit is pushed to the [allwpilib](https://github.com/wpilibsuite/allwpilib/) repository. These builds are then hosted on [artifactory](https://frcmaven.wpi.edu/artifactory/webapp/#/home).
+
+In order to build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version.
+
+```groovy
+wpi.maven.useDevelopment = true
+wpi.wpilibVersion = 'YEAR.+'
+```
+
+The top of your ``build.gradle`` file should now look similar to the code below. Ignore any differences in versions.
+
+Java
+```groovy
+plugins {
+  id "java"
+  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+}
+
+wpi.maven.useDevelopment = true
+wpi.wpilibVersion = '2020.+'
+```
+
+C++
+```groovy
+plugins {
+  id "cpp"
+  id "google-test-test-suite"
+  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+}
+
+wpi.maven.useDevelopment = true
+wpi.wpilibVersion = '2020.+'
+```
+
+## Local Build
+
+Building with a local build is very similar to building with a development build. Ensure you have built and published WPILib by following the instructions attached [here](https://github.com/wpilibsuite/allwpilib#building-wpilib). Next, find the ``build.gradle`` file in your robot project and open it. Then, add the following code below the plugin section and replace ``YEAR`` with the year of the local version.
+
+Java
+```groovy
+plugins {
+  id "java"
+  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+}
+
+wpi.maven.useFrcMavenLocalDevelopment = true
+wpi.wpilibVersion = 'YEAR.424242.+'
+```
+
+C++
+```groovy
+plugins {
+  id "cpp"
+  id "google-test-test-suite"
+  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+}
+
+wpi.maven.useFrcMavenLocalDevelopment = true
+wpi.wpilibVersion = 'YEAR.424242.+'
+```

README-CMAKE.md

diff --git a/README-CMAKE.md b/README-CMAKE.md
index 68e8175..09c9792 100644
--- a/README-CMAKE.md
+++ b/README-CMAKE.md
@@ -1,6 +1,6 @@
 # WPILib CMake Support
 
-WPILib is normally built with Gradle, however for some systems, such a linux based coprocessors, Gradle doesn't work correctly, especially if cscore is needed, which requires OpenCV. We provide the CMake build for these cases. Although it is supported on Windows, these docs will only go over linux builds.
+WPILib is normally built with Gradle, however for some systems, such as Linux based coprocessors, Gradle doesn't work correctly, especially if cscore is needed, which requires OpenCV. Furthermore, the CMake build can be used for C++ development because it provides better build caching compared to Gradle. We provide the CMake build for these cases. Although it is supported on Windows, these docs will only go over Linux builds.
 
 ## Libraries that get built
 * wpiutil
@@ -9,56 +9,67 @@
 * cameraserver
 * hal
 * wpilib
+* halsim
+* wpigui
+* wpimath
 
-By default, all libraries except for the HAL and WPILib get built with a default cmake setup. The libraries are built as shared libraries, and include the JNI libraries as well as building the Java jars.
+By default, all libraries except for the HAL and WPILib get built with a default CMake setup. The libraries are built as shared libraries, and include the JNI libraries as well as building the Java JARs.
 
 ## Prerequisites
 
-The most common prerequisite is going to be OpenCV. OpenCV needs to be findable by cmake. On systems like the Jetson, this is installed by default. Otherwise, you will need to build cmake from source and install it.
+The most common prerequisite is going to be OpenCV. OpenCV needs to be findable by CMake. On systems like the Jetson, this is installed by default. Otherwise, you will need to build OpenCV from source and install it.
 
-In addition, if you want JNI and Java, you will need a JDK of at least version 8 installed. In addition, you need a `JAVA_HOME` environment variable set properly and set to the jdk directory.
+In addition, if you want JNI and Java, you will need a JDK of at least version 11 installed. In addition, you need a `JAVA_HOME` environment variable set properly and set to the JDK directory.
+
+If you are building with unit tests or simulation modules, you will also need an Internet connection for the initial setup process, as CMake will clone google-test and imgui from GitHub.
 
 ## Build Options
 
 The following build options are available:
 
-* WITHOUT_JAVA (OFF Default)
-  * Enabling this option will disable Java and JNI builds. If this is off, `BUILD_SHARED_LIBS` must be on. Otherwise cmake will error.
-* BUILD_SHARED_LIBS (ON Default)
-  * Disabling this option will cause cmake to build static libraries instead of shared libraries. If this is off, `WITHOUT_JAVA` must be on. Otherwise cmake will error.
-* WITHOUT_CSCORE (OFF Default)
-  * Enabling this option will cause cscore to not be built. This will also implicitly disable cameraserver, the hal and wpilib as well, irrespective of their specific options. If this is on, the opencv build requirement is removed.
-* WITHOUT_ALLWPILIB (ON Default)
-  * Disabling this option will build the hal and wpilib during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO.
-* USE_EXTERNAL_HAL (OFF Default)
+* `WITH_JAVA` (ON Default)
+  * This option will enable Java and JNI builds. If this is on, `WITH_SHARED_LIBS` must be on. Otherwise CMake will error.
+* `WITH_SHARED_LIBS` (ON Default)
+  * This option will cause cmake to build static libraries instead of shared libraries. If this is off, `WITH_JAVA` must be off. Otherwise CMake will error.
+* `WITH_TESTS` (ON Default)
+  * This option will build C++ unit tests. These can be run via `make test`.
+* `WITH_CSCORE` (ON Default)
+  * This option will cause cscore to be built. Turning this off will implicitly disable cameraserver, the hal and wpilib as well, irrespective of their specific options. If this is off, the OpenCV build requirement is removed.
+* `WITH_WPILIB` (ON Default)
+  * This option will build the hal, wpilibc/j, and wpimath during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO.
+* `WITH_SIMULATION_MODULES` (ON Default)
+  * This option will build simulation modules, including wpigui and the HALSim plugins.
+* `WITH_EXTERNAL_HAL` (OFF Default)
   * TODO
-* EXTERNAL_HAL_FILE
+* `EXTERNAL_HAL_FILE`
   * TODO
+* `OPENCV_JAVA_INSTALL_DIR`
+  * Set this option to the location of the archive of the OpenCV Java bindings (it should be called opencv-xxx.jar, with the x'es being version numbers). NOTE: set it to the LOCATION of the file, not the file itself!
 
 ## Build Setup
 
-The WPILib CMake build does not allow in source builds. Because the `build` directory is used by gradle, we recommend a `buildcmake` directory in the root. This folder is included in the gitignore.
+The WPILib CMake build does not allow in source builds. Because the `build` directory is used by Gradle, we recommend a `buildcmake` directory in the root. This folder is included in the gitignore.
 
-Once you have a build folder, run cmake configuration in that build directory with the following command.
+Once you have a build folder, run CMake configuration in that build directory with the following command.
 
 ```
 cmake path/to/allwpilib/root
 ```
 
-If you want to change any of the options, add `-DOPTIONHERE=VALUE` to the cmake command. This will check for any dependencies. If everything works properly this will succeed. If not, please check out the troubleshooting section for help.
+If you want to change any of the options, add `-DOPTIONHERE=VALUE` to the `cmake` command. This will check for any dependencies. If everything works properly this will succeed. If not, please check out the troubleshooting section for help.
 
-If you want, you can also use `ccmake` in order to visually set these properties as well.
-https://cmake.org/cmake/help/v3.0/manual/ccmake.1.html
-Here is the link to the documentation for that program.
+If you want, you can also use `ccmake` in order to visually set these properties as well. [Here](https://cmake.org/cmake/help/v3.0/manual/ccmake.1.html) is the link to the documentation for that program.
 
 ## Building
 
-Once you have cmake setup. run `make` from the directory you configured cmake in. This will build all libraries possible. If you have a multicore system, we recommend running make with multiple jobs. The usual rule of thumb is 1.5x the number of cores you have. To run a multiple job build, run the following command with x being the number of jobs you want.
+Once you have cmake setup. run `make` from the directory you configured CMake in. This will build all libraries possible. If you have a multicore system, we recommend running `make` with multiple jobs. The usual rule of thumb is 1.5x the number of cores you have. To run a multiple job build, run the following command with x being the number of jobs you want.
 
 ```
 make -jx
 ```
 
+The `ninja` generator is also supported, and can be enabled by passing `-GNinja` to the initial `cmake` command.
+
 ## Installing
 
 After build, the easiest way to use the libraries is to install them. Run the following command to install the libraries. This will install them so that they can be used from external cmake projects.
@@ -79,7 +90,7 @@
 
 find_package(wpilib REQUIRED)
 
-add_executable(my_vision_app main.cpp) # exectuable name as first parameter
+add_executable(my_vision_app main.cpp) # executable name as first parameter
 target_link_libraries(my_vision_app cameraserver ntcore cscore wpiutil)
 ```
 
@@ -135,4 +146,4 @@
 
 If this happens, make sure you have a JDK of at least version 8 installed, and that your JAVA_HOME variable is set properly to point to the JDK.
 
-In addition, if you do not need Java, you can disable it with `-DWITHOUT_JAVA=ON`.
+In addition, if you do not need Java, you can disable it with `-DWITH_JAVA=OFF`.

README.md

diff --git a/README.md b/README.md
index fe1f8c5..836924f 100644
--- a/README.md
+++ b/README.md
@@ -1,21 +1,28 @@
 # WPILib Project
 
-[![Build Status](https://dev.azure.com/wpilib/wpilib/_apis/build/status/wpilibsuite.allwpilib)](https://dev.azure.com/wpilib/wpilib/_build/latest?definitionId=1)
+![CI](https://github.com/wpilibsuite/allwpilib/workflows/CI/badge.svg)
 
 Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WPILibC projects. These are the core libraries for creating robot programs for the roboRIO.
 
-- [WPILib Mission](#wpilib-mission)
+- [WPILib Project](#wpilib-project)
+  - [WPILib Mission](#wpilib-mission)
 - [Building WPILib](#building-wpilib)
-    - [Requirements](#requirements)
-    - [Setup](#setup)
-    - [Building](#building)
-    - [Publishing](#publishing)
-    - [Structure and Organization](#structure-and-organization)
+  - [Requirements](#requirements)
+  - [Setup](#setup)
+  - [Building](#building)
+    - [Faster builds](#faster-builds)
+    - [Using Development Builds](#using-development-builds)
+    - [Custom toolchain location](#custom-toolchain-location)
+    - [Gazebo simulation](#gazebo-simulation)
+    - [Formatting/linting with wpiformat](#formattinglinting-with-wpiformat)
+    - [CMake](#cmake)
+  - [Publishing](#publishing)
+  - [Structure and Organization](#structure-and-organization)
 - [Contributing to WPILib](#contributing-to-wpilib)
 
 ## WPILib Mission
 
-The WPILib Mission is to enable FIRST Robotics teams to focus on writing game-specific software rather than focusing on hardware details - "raise the floor, don't lower the ceiling". We work to enable teams with limited programming knowledge and/or mentor experience to be as successful as possible, while not hampering the abilities of teams with more advanced programming capabilities. We support Kit of Parts control system components directly in the library. We also strive to keep parity between major features of each language (Java, C++, and NI's LabVIEW), so that teams aren't at a disadvantage for choosing a specific programming language. WPILib is an open source project, licensed under the BSD 3-clause license. You can find a copy of the license [here](LICENSE.txt).
+The WPILib Mission is to enable FIRST Robotics teams to focus on writing game-specific software rather than focusing on hardware details - "raise the floor, don't lower the ceiling". We work to enable teams with limited programming knowledge and/or mentor experience to be as successful as possible, while not hampering the abilities of teams with more advanced programming capabilities. We support Kit of Parts control system components directly in the library. We also strive to keep parity between major features of each language (Java, C++, and NI's LabVIEW), so that teams aren't at a disadvantage for choosing a specific programming language. WPILib is an open source project, licensed under the BSD 3-clause license. You can find a copy of the license [here](LICENSE.md).
 
 # Building WPILib
 
@@ -23,13 +30,13 @@
 
 ## Requirements
 
-- A C++ compiler
-    - On Linux, GCC works fine
-    - On Windows, you need Visual Studio 2019 (the free community edition works fine).
-      Make sure to select the C++ Programming Language for installation
-- [ARM Compiler Toolchain](https://github.com/wpilibsuite/toolchain-builder/releases)
-  * Note that for 2020 and beyond, you should use version 7 or greater of GCC
-- Doxygen (Only required if you want to build the C++ documentation)
+- [JDK 11](https://adoptopenjdk.net/)
+- C++ compiler
+    - On Linux, install GCC
+    - On Windows, install [Visual Studio Community 2019](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio 2019)
+    - On macOS, install the Xcode command-line build tools via `xcode-select --install`
+- [ARM compiler toolchain](https://github.com/wpilibsuite/roborio-toolchain/releases)
+    - For 2020 and beyond, use GCC version 7 or greater
 
 ## Setup
 
@@ -51,12 +58,34 @@
 ./gradlew :wpilibc:build
 ```
 
+The gradlew wrapper only exists in the root of the main project, so be sure to run all commands from there. All of the subprojects have build tasks that can be run. Gradle automatically determines and rebuilds dependencies, so if you make a change in the HAL and then run `./gradlew :wpilibc:build`, the HAL will be rebuilt, then WPILibC.
+
+There are a few tasks other than `build` available. To see them, run the meta-task `tasks`. This will print a list of all available tasks, with a description of each task.
+
+### Faster builds
+
+`./gradlew build` builds _everything_, which includes debug and release builds for desktop and all installed cross compilers. Many developers don't need or want to build all of this. Therefore, common tasks have shortcuts to only build necessary components for common development and testing tasks.
+
+`./gradlew testDesktopCpp` and `./gradlew testDesktopJava` will build and run the tests for `wpilibc` and `wpilibj` respectively. They will only build the minimum components required to run the tests.
+
+`testDesktopCpp` and `testDesktopJava` tasks also exist for the projects `wpiutil`, `ntcore`, `cscore`, `hal` `wpilibOldCommands`, `wpilibNewCommands` and `cameraserver`. These can be ran with `./gradlew :projectName:task`.
+
+`./gradlew buildDesktopCpp` and `./gradlew buildDesktopJava` will compile `wpilibcExamples` and `wpilibjExamples` respectively. The results can't be ran, but they can compile.
+
+### Using Development Builds
+
+Please read the documentation available [here](OtherVersions.md)
+
+### Custom toolchain location
+
 If you have installed the FRC Toolchain to a directory other than the default, or if the Toolchain location is not on your System PATH, you can pass the `toolChainPath` property to specify where it is located. Example:
 
 ```bash
 ./gradlew build -PtoolChainPath=some/path/to/frc/toolchain/bin
 ```
 
+### Gazebo simulation
+
 If you also want simulation to be built, add -PmakeSim. This requires gazebo_transport. We have tested on 14.04 and 15.05, but any correct install of Gazebo should work, even on Windows if you build Gazebo from source. Correct means CMake needs to be able to find gazebo-config.cmake. See [The Gazebo website](https://gazebosim.org/) for installation instructions.
 
 ```bash
@@ -73,12 +102,19 @@
 make
 ```
 
-The gradlew wrapper only exists in the root of the main project, so be sure to run all commands from there. All of the subprojects have build tasks that can be run. Gradle automatically determines and rebuilds dependencies, so if you make a change in the HAL and then run `./gradlew :wpilibc:build`, the HAL will be rebuilt, then WPILibC.
 
-There are a few tasks other than `build` available. To see them, run the meta-task `tasks`. This will print a list of all available tasks, with a description of each task.
+### Formatting/linting
+
+#### wpiformat
 
 wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat` on Windows or `python3 -m wpiformat` on other platforms.
 
+#### Java Code Quality Tools
+
+The Java code quality tools (checkstyle, pmd, etc.) can be run with the `./gradlew javaFormat` task.
+
+### CMake
+
 CMake is also supported for building. See [README-CMAKE.md](README-CMAKE.md).
 
 ## Publishing

ThirdPartyNotices.txt

diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt
index fae1250..55d21f3 100644
--- a/ThirdPartyNotices.txt
+++ b/ThirdPartyNotices.txt
@@ -36,14 +36,18 @@
 Feather Icons         wpiutil/src/main/native/resources/feather-*
 jQuery                wpiutil/src/main/native/resources/jquery-*
 popper.js             wpiutil/src/main/native/resources/popper-*
-units                 wpiutil/src/main/native/include/units/units.h
-Eigen                 wpiutil/src/main/native/eigeninclude/
+units                 wpimath/src/main/native/include/units/
+Eigen                 wpimath/src/main/native/eigeninclude/
+                      wpimath/src/main/native/include/unsupported/
 StackWalker           wpiutil/src/main/native/windows/StackWalker.*
 Team 254 Library      wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
                       wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
                       wpilibc/src/main/native/include/spline/SplineParameterizer.h
                       wpilibc/src/main/native/include/trajectory/TrajectoryParameterizer.h
                       wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+Portable File Dialogs wpigui/src/main/native/include/portable-file-dialogs.h
+Drake                 wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
+                      wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
 
 
 ==============================================================================
@@ -806,3 +810,38 @@
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
+
+=============
+Drake Library
+=============
+All components of Drake are licensed under the BSD 3-Clause License
+shown below. Where noted in the source code, some portions may
+be subject to other permissive, non-viral licenses.
+
+Copyright 2012-2016 Robot Locomotion Group @ CSAIL
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.  Redistributions
+in binary form must reproduce the above copyright notice, this list of
+conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.  Neither the name of
+the Massachusetts Institute of Technology nor the names of its
+contributors may be used to endorse or promote products derived from
+this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

azure-pipelines-testbench.yaml

diff --git a/azure-pipelines-testbench.yaml b/azure-pipelines-testbench.yaml
index 67cd75c..674926e 100644
--- a/azure-pipelines-testbench.yaml
+++ b/azure-pipelines-testbench.yaml
@@ -15,7 +15,7 @@
       vmImage: 'Ubuntu 16.04'
 
     container:
-      image:  wpilib/roborio-cross-ubuntu:2020-18.04
+      image:  wpilib/roborio-cross-ubuntu:2021-18.04
 
     timeoutInMinutes: 0
 

azure-pipelines.yml

diff --git a/azure-pipelines.yml b/azure-pipelines.yml
deleted file mode 100644
index 2c5e952..0000000
--- a/azure-pipelines.yml
+++ /dev/null
@@ -1,417 +0,0 @@
-# Gradle
-# Build your Java projects and run tests with Gradle using a Gradle wrapper script.
-# Add steps that analyze code, save build artifacts, deploy, and more:
-# https://docs.microsoft.com/vsts/pipelines/languages/java
-
-resources:
-  containers:
-  - container: wpilib2020
-    image: wpilib/roborio-cross-ubuntu:2020-18.04
-  - container: raspbian
-    image:  wpilib/raspbian-cross-ubuntu:10-18.04
-  - container: aarch64
-    image:  wpilib/aarch64-cross-ubuntu:bionic-18.04
-  - container: ubuntu
-    image:  wpilib/ubuntu-base:18.04
-
-variables:
-- group: Artifactory-Package-Publish
-
-trigger:
-  batch: true
-  branches:
-    include:
-    - master
-
-stages:
-- stage: Build
-  jobs:
-  - job: Linux_Arm
-    pool:
-      vmImage: 'Ubuntu 16.04'
-
-    container: wpilib2020
-
-    timeoutInMinutes: 0
-
-    steps:
-      - task: Gradle@2
-        condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: false
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-Ponlylinuxathena -PbuildServer'
-
-      - task: Gradle@2
-        condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: false
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-Ponlylinuxathena -PreleaseMode -PbuildServer'
-
-      - task: PublishPipelineArtifact@0
-        inputs:
-          artifactName: 'Athena'
-          targetPath: 'build/allOutputs'
-
-  - job: Linux_Raspbian
-    pool:
-      vmImage: 'Ubuntu 16.04'
-
-    container: raspbian
-
-    timeoutInMinutes: 0
-
-    steps:
-      - task: Gradle@2
-        condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-Ponlylinuxraspbian -PbuildServer'
-
-      - task: Gradle@2
-        condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-Ponlylinuxraspbian -PreleaseMode -PbuildServer'
-
-      - task: PublishPipelineArtifact@0
-        inputs:
-          artifactName: 'Raspbian'
-          targetPath: 'build/allOutputs'
-
-  - job: Linux_Aarch64
-    pool:
-      vmImage: 'Ubuntu 16.04'
-
-    container: aarch64
-
-    timeoutInMinutes: 0
-
-    steps:
-      - task: Gradle@2
-        condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-Ponlylinuxaarch64bionic -PbuildServer'
-
-      - task: Gradle@2
-        condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-Ponlylinuxaarch64bionic -PreleaseMode -PbuildServer'
-
-      - task: PublishPipelineArtifact@0
-        inputs:
-          artifactName: 'Aarch64'
-          targetPath: 'build/allOutputs'
-
-  - job: Linux
-    pool:
-      vmImage: 'Ubuntu 16.04'
-
-    container: ubuntu
-
-    timeoutInMinutes: 0
-
-    steps:
-      - task: Gradle@2
-        condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PbuildServer'
-
-      - task: Gradle@2
-        condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PreleaseMode -PbuildServer'
-
-      - task: PublishPipelineArtifact@0
-        inputs:
-          artifactName: 'Linux'
-          targetPath: 'build/allOutputs'
-
-  - job: Styleguide
-    pool:
-      vmImage: 'Ubuntu 16.04'
-
-    container: ubuntu
-
-    timeoutInMinutes: 0
-
-    steps:
-        - script: |
-            sudo pip3 install wpiformat
-          displayName: 'Install wpiformat'
-        - script: |
-            git checkout -b master
-            wpiformat -clang 6.0
-          displayName: 'Run wpiformat'
-        - script: |
-            # Ensure formatter made no changes
-            git --no-pager diff --exit-code HEAD
-          displayName: 'Check wpiformat Output'
-
-  - job: CMakeBuild
-    pool:
-      vmImage: 'Ubuntu 16.04'
-
-    container: wpilib2020
-
-    timeoutInMinutes: 0
-
-    steps:
-        - task: CMake@1
-          inputs:
-            cmakeArgs: '-DWITHOUT_ALLWPILIB=OFF ..'
-        - script: |
-            make -j3
-          workingDirectory: 'build'
-          displayName: 'Build'
-
-  - job: Windows_64_Bit
-    pool:
-      vmImage: 'windows-2019'
-
-    timeoutInMinutes: 0
-    steps:
-      - task: Gradle@2
-        condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          jdkVersionOption: '1.11'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PskipPMD -PbuildServer'
-
-      - task: Gradle@2
-        condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          jdkVersionOption: '1.11'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PskipPMD -PreleaseMode -PbuildServer'
-
-      - task: PublishPipelineArtifact@0
-        inputs:
-          artifactName: 'Win64'
-          targetPath: 'build/allOutputs'
-
-  - job: Windows_32_Bit
-    pool:
-      vmImage: 'windows-2019'
-
-    timeoutInMinutes: 0
-    steps:
-      - powershell: |
-          mkdir build
-          $ProgressPreference = 'SilentlyContinue'
-          wget "https://github.com/AdoptOpenJDK/openjdk11-binaries/releases/download/jdk-11.0.4%2B11/OpenJDK11U-jdk_x86-32_windows_hotspot_11.0.4_11.zip" -O "build\jdk.zip"
-        displayName: 'Download JDK'
-      - task: JavaToolInstaller@0
-        inputs:
-          jdkSourceOption: localDirectory
-          jdkFile: 'build/jdk.zip'
-          jdkDestinationDirectory: 'build/jdkinst'
-          jdkArchitectureOption: x86
-
-      - task: Gradle@2
-        condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx1024m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PskipPMD -PbuildServer'
-
-      - task: Gradle@2
-        condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx1024m'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PskipPMD -PreleaseMode -PbuildServer'
-
-      - task: PublishPipelineArtifact@0
-        inputs:
-          artifactName: 'Win32'
-          targetPath: 'build/allOutputs'
-
-  - job: Mac
-    pool:
-      vmImage: 'macOS-10.14'
-
-    timeoutInMinutes: 0
-    steps:
-      - script: |
-          mkdir build
-          export JAVA_HOME=`/usr/libexec/java_home -v 11`
-        displayName: 'Setup JDK'
-
-      - task: Gradle@2
-        condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          jdkVersionOption: '1.11'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PbuildServer'
-
-      - task: Gradle@2
-        condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-        inputs:
-          workingDirectory: ''
-          gradleWrapperFile: 'gradlew'
-          gradleOptions: '-Xmx3072m'
-          jdkVersionOption: '1.11'
-          publishJUnitResults: true
-          testResultsFiles: '**/TEST-*.xml'
-          tasks: 'build'
-          options: '-PreleaseMode -PbuildServer'
-
-      - task: PublishPipelineArtifact@0
-        inputs:
-          artifactName: 'Mac'
-          targetPath: 'build/allOutputs'
-
-- stage: Combine
-  jobs:
-  - job: CombineJob
-    pool:
-      vmImage: 'macOS-10.14'
-
-    timeoutInMinutes: 0
-
-    steps:
-    - checkout: none
-    - script: |
-        git clone https://github.com/wpilibsuite/build-tools
-      displayName: 'Clone Combiner'
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Mac'
-        targetPath: 'build-tools/combiner/products/build/allOutputs'
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Win32'
-        targetPath: 'build-tools/combiner/products/build/allOutputs'
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Win64'
-        targetPath: 'build-tools/combiner/products/build/allOutputs'
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Linux'
-        targetPath: 'build-tools/combiner/products/build/allOutputs'
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Raspbian'
-        targetPath: 'build-tools/combiner/products/build/allOutputs'
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Athena'
-        targetPath: 'build-tools/combiner/products/build/allOutputs'
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Aarch64'
-        targetPath: 'build-tools/combiner/products/build/allOutputs'
-
-# PR Builds
-    - task: Gradle@2
-      inputs:
-        workingDirectory: 'build-tools/combiner'
-        gradleWrapperFile: 'build-tools/combiner/gradlew'
-        gradleOptions: '-Xmx3072m'
-        tasks: 'publish '
-        options: '-Pallwpilib'
-      condition: and(succeeded(), and(ne(variables['Build.SourceBranch'], 'refs/heads/master'), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))))
-
-# Master Builds
-    - task: Gradle@2
-      inputs:
-        workingDirectory: 'build-tools/combiner'
-        gradleWrapperFile: 'build-tools/combiner/gradlew'
-        gradleOptions: '-Xmx3072m'
-        tasks: 'publish '
-        options: '-Pallwpilib'
-      condition: and(succeeded(), eq(variables['Build.SourceBranch'], 'refs/heads/master'))
-      env:
-        RUN_AZURE_ARTIFACTORY_RELEASE: 'TRUE'
-        ARTIFACTORY_PUBLISH_USERNAME: $(PublishUserName)
-        ARTIFACTORY_PUBLISH_PASSWORD: $(PublishPassword)
-
-# Tagged Builds
-    - task: Gradle@2
-      inputs:
-        workingDirectory: 'build-tools/combiner'
-        gradleWrapperFile: 'build-tools/combiner/gradlew'
-        gradleOptions: '-Xmx3072m'
-        tasks: 'publish '
-        options: '-Pallwpilib -PreleaseRepoPublish'
-      condition: and(succeeded(), startsWith(variables['Build.SourceBranch'], 'refs/tags/v'))
-      env:
-        RUN_AZURE_ARTIFACTORY_RELEASE: 'TRUE'
-        ARTIFACTORY_PUBLISH_USERNAME: $(PublishUserName)
-        ARTIFACTORY_PUBLISH_PASSWORD: $(PublishPassword)
-
-    - script: |
-        echo "##vso[task.setvariable variable=UserHome]$HOME"
-      displayName: 'Set Home Variable'
-    - task: PublishPipelineArtifact@0
-      inputs:
-        artifactName: 'Maven'
-        targetPath: $(UserHome)/releases

build.gradle

diff --git a/build.gradle b/build.gradle
index 81b333f..a5c0577 100644
--- a/build.gradle
+++ b/build.gradle
@@ -2,7 +2,7 @@
 
 plugins {
     id 'base'
-    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '4.0.1'
+    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '4.0.2'
     id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2'
     id 'edu.wpi.first.NativeUtils' apply false
     id 'edu.wpi.first.GradleJni' version '0.10.1'
@@ -15,10 +15,12 @@
 
 if (project.hasProperty('buildServer')) {
     wpilibVersioning.buildServerMode = true
+    wpilibVersioning.useAllTags = true
 }
 
 if (project.hasProperty('releaseMode')) {
     wpilibVersioning.releaseMode = true
+    wpilibVersioning.useAllTags = true
 }
 
 allprojects {
@@ -39,7 +41,7 @@
     publishAlways()
 }
 
-ext.licenseFile = files("$rootDir/LICENSE.txt", "$rootDir/ThirdPartyNotices.txt")
+ext.licenseFile = files("$rootDir/LICENSE.md", "$rootDir/ThirdPartyNotices.txt")
 
 if (project.hasProperty("publishVersion")) {
     wpilibVersioning.version.set(project.publishVersion)
@@ -103,6 +105,26 @@
             }
         }
     }
+
+    // Sign outputs with Developer ID
+    if (project.hasProperty("developerID")) {
+        tasks.withType(AbstractLinkTask) { task ->
+            // Don't sign any executables because codesign complains
+            // about relative rpath.
+            if (!(task instanceof LinkExecutable)) {
+                doLast {
+                    // Get path to binary.
+                    String path = task.getLinkedFile().getAsFile().get().getAbsolutePath()
+                    exec {
+                        workingDir rootDir
+                        def args = ["sh", "-c", "codesign --force --strict --timestamp --options=runtime " +
+                                    "--verbose -s ${project.findProperty("developerID")} ${path}"]
+                        commandLine args
+                    }
+                }
+            }
+        }
+    }
 }
 
 ext.getCurrentArch = {
@@ -110,5 +132,5 @@
 }
 
 wrapper {
-    gradleVersion = '6.0'
+    gradleVersion = '6.0.1'
 }

buildSrc/build.gradle

diff --git a/buildSrc/build.gradle b/buildSrc/build.gradle
index b4a6a01..e4b3a6d 100644
--- a/buildSrc/build.gradle
+++ b/buildSrc/build.gradle
@@ -5,5 +5,5 @@
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2020.7.2"
+    implementation "edu.wpi.first:native-utils:2021.0.4"
 }

buildSrc/src/main/groovy/SingleNativeBuild.groovy

diff --git a/buildSrc/src/main/groovy/SingleNativeBuild.groovy b/buildSrc/src/main/groovy/SingleNativeBuild.groovy
index 2b7e087..d43afff 100644
--- a/buildSrc/src/main/groovy/SingleNativeBuild.groovy
+++ b/buildSrc/src/main/groovy/SingleNativeBuild.groovy
@@ -10,6 +10,8 @@
 import org.gradle.api.Plugin;
 import org.gradle.api.Project;
 import org.gradle.api.Task;
+import org.gradle.api.tasks.Copy;
+import org.gradle.api.file.CopySpec;
 import org.gradle.api.file.FileTree;
 import org.gradle.api.tasks.compile.JavaCompile;
 import org.gradle.language.base.internal.ProjectLayout;
@@ -32,6 +34,8 @@
 import org.gradle.nativeplatform.toolchain.internal.gcc.AbstractGccCompatibleToolChain;
 import org.gradle.nativeplatform.toolchain.internal.msvcpp.VisualCppToolChain;
 import org.gradle.nativeplatform.toolchain.internal.tools.ToolRegistry;
+import org.gradle.nativeplatform.tasks.CreateStaticLibrary;
+import org.gradle.nativeplatform.tasks.AbstractLinkTask;
 import org.gradle.platform.base.BinarySpec;
 import org.gradle.platform.base.ComponentSpec;
 import org.gradle.platform.base.ComponentSpecContainer;
@@ -79,7 +83,7 @@
             components.each { component ->
                 if (component.name == "${nativeName}Base") {
                     base = (NativeLibrarySpec) component
-                } else if (component.name == "${nativeName}" || component.name == "${nativeName}JNI") {
+                } else if (component.name == "${nativeName}" || component.name == "${nativeName}JNI" || component.name == "${nativeName}JNICvStatic") {
                     subs << component
                 }
             }
@@ -100,6 +104,23 @@
                             baseBin = tmpBaseBin
                         }
                     }
+
+                    if (binary instanceof StaticLibraryBinarySpec) {
+                        File intoDir = ((CreateStaticLibrary)((StaticLibraryBinarySpec)binary).tasks.createStaticLib).outputFile.get().asFile.parentFile
+                        File fromDir = ((CreateStaticLibrary)((StaticLibraryBinarySpec)baseBin).tasks.createStaticLib).outputFile.get().asFile.parentFile
+
+                        def copyBasePdbName = "copyBasePdbFor" + binary.buildTask.name
+                        def copyTask = project.tasks.register(copyBasePdbName, Copy) { Copy t ->
+                            t.from (fromDir)
+                            t.include '*.pdb'
+                            t.into intoDir
+
+                            t.dependsOn (((StaticLibraryBinarySpec)baseBin).tasks.createStaticLib)
+                        }
+                        ((CreateStaticLibrary)((StaticLibraryBinarySpec)binary).tasks.createStaticLib).dependsOn(copyTask)
+
+                    }
+
                     baseBin.tasks.withType(AbstractNativeSourceCompileTask) { oCompileTask ->
                         def compileTask = (AbstractNativeSourceCompileTask) oCompileTask
                         if (binary instanceof SharedLibraryBinarySpec) {

cameraserver/CMakeLists.txt

diff --git a/cameraserver/CMakeLists.txt b/cameraserver/CMakeLists.txt
index d77cb4e..33e19d0 100644
--- a/cameraserver/CMakeLists.txt
+++ b/cameraserver/CMakeLists.txt
@@ -6,7 +6,7 @@
 find_package( OpenCV REQUIRED )
 
 # Java bindings
-if (NOT WITHOUT_JAVA)
+if (WITH_JAVA)
     find_package(Java REQUIRED)
     include(UseJava)
     set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
@@ -43,11 +43,11 @@
 install(TARGETS cameraserver EXPORT cameraserver DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cameraserver")
 
-if (NOT WITHOUT_JAVA AND MSVC)
+if (WITH_JAVA AND MSVC)
     install(TARGETS cameraserver RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
 endif()
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
     set (cameraserver_config_dir ${wpilib_dest})
 else()
     set (cameraserver_config_dir share/cameraserver)

cameraserver/multiCameraServer/build.gradle

diff --git a/cameraserver/multiCameraServer/build.gradle b/cameraserver/multiCameraServer/build.gradle
index 88a6f0e..e8565cd 100644
--- a/cameraserver/multiCameraServer/build.gradle
+++ b/cameraserver/multiCameraServer/build.gradle
@@ -18,7 +18,7 @@
 
 apply from: "${rootDir}/shared/opencv.gradle"
 
-mainClassName = 'Main'
+mainClassName = 'edu.wpi.Main'
 
 apply plugin: 'com.github.johnrengelman.shadow'
 

cameraserver/multiCameraServer/src/main/java/Main.java

diff --git a/cameraserver/multiCameraServer/src/main/java/Main.java b/cameraserver/multiCameraServer/src/main/java/Main.java
deleted file mode 100644
index 8bcc54d..0000000
--- a/cameraserver/multiCameraServer/src/main/java/Main.java
+++ /dev/null
@@ -1,211 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Paths;
-import java.util.ArrayList;
-import java.util.List;
-
-import com.google.gson.Gson;
-import com.google.gson.GsonBuilder;
-import com.google.gson.JsonArray;
-import com.google.gson.JsonElement;
-import com.google.gson.JsonObject;
-import com.google.gson.JsonParser;
-
-import edu.wpi.cscore.VideoSource;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-/*
-   JSON format:
-   {
-       "team": <team number>,
-       "ntmode": <"client" or "server", "client" if unspecified>
-       "cameras": [
-           {
-               "name": <camera name>
-               "path": <path, e.g. "/dev/video0">
-               "pixel format": <"MJPEG", "YUYV", etc>   // optional
-               "width": <video mode width>              // optional
-               "height": <video mode height>            // optional
-               "fps": <video mode fps>                  // optional
-               "brightness": <percentage brightness>    // optional
-               "white balance": <"auto", "hold", value> // optional
-               "exposure": <"auto", "hold", value>      // optional
-               "properties": [                          // optional
-                   {
-                       "name": <property name>
-                       "value": <property value>
-                   }
-               ]
-           }
-       ]
-   }
- */
-
-public final class Main {
-  private static String configFile = "/boot/frc.json";
-
-  @SuppressWarnings("MemberName")
-  public static class CameraConfig {
-    public String name;
-    public String path;
-    public JsonObject config;
-  }
-
-  public static int team;
-  public static boolean server;
-  public static List<CameraConfig> cameras = new ArrayList<>();
-
-  private Main() {
-  }
-
-  /**
-   * Report parse error.
-   */
-  public static void parseError(String str) {
-    System.err.println("config error in '" + configFile + "': " + str);
-  }
-
-  /**
-   * Read single camera configuration.
-   */
-  public static boolean readCameraConfig(JsonObject config) {
-    CameraConfig cam = new CameraConfig();
-
-    // name
-    JsonElement nameElement = config.get("name");
-    if (nameElement == null) {
-      parseError("could not read camera name");
-      return false;
-    }
-    cam.name = nameElement.getAsString();
-
-    // path
-    JsonElement pathElement = config.get("path");
-    if (pathElement == null) {
-      parseError("camera '" + cam.name + "': could not read path");
-      return false;
-    }
-    cam.path = pathElement.getAsString();
-
-    cam.config = config;
-
-    cameras.add(cam);
-    return true;
-  }
-
-  /**
-   * Read configuration file.
-   */
-  @SuppressWarnings("PMD.CyclomaticComplexity")
-  public static boolean readConfig() {
-    // parse file
-    JsonElement top;
-    try {
-      top = new JsonParser().parse(Files.newBufferedReader(Paths.get(configFile)));
-    } catch (IOException ex) {
-      System.err.println("could not open '" + configFile + "': " + ex);
-      return false;
-    }
-
-    // top level must be an object
-    if (!top.isJsonObject()) {
-      parseError("must be JSON object");
-      return false;
-    }
-    JsonObject obj = top.getAsJsonObject();
-
-    // team number
-    JsonElement teamElement = obj.get("team");
-    if (teamElement == null) {
-      parseError("could not read team number");
-      return false;
-    }
-    team = teamElement.getAsInt();
-
-    // ntmode (optional)
-    if (obj.has("ntmode")) {
-      String str = obj.get("ntmode").getAsString();
-      if ("client".equalsIgnoreCase(str)) {
-        server = false;
-      } else if ("server".equalsIgnoreCase(str)) {
-        server = true;
-      } else {
-        parseError("could not understand ntmode value '" + str + "'");
-      }
-    }
-
-    // cameras
-    JsonElement camerasElement = obj.get("cameras");
-    if (camerasElement == null) {
-      parseError("could not read cameras");
-      return false;
-    }
-    JsonArray cameras = camerasElement.getAsJsonArray();
-    for (JsonElement camera : cameras) {
-      if (!readCameraConfig(camera.getAsJsonObject())) {
-        return false;
-      }
-    }
-
-    return true;
-  }
-
-  /**
-   * Start running the camera.
-   */
-  public static void startCamera(CameraConfig config) {
-    System.out.println("Starting camera '" + config.name + "' on " + config.path);
-    VideoSource camera = CameraServer.getInstance().startAutomaticCapture(
-        config.name, config.path);
-
-    Gson gson = new GsonBuilder().create();
-
-    camera.setConfigJson(gson.toJson(config.config));
-  }
-
-  /**
-   * Main.
-   */
-  public static void main(String... args) {
-    if (args.length > 0) {
-      configFile = args[0];
-    }
-
-    // read configuration
-    if (!readConfig()) {
-      return;
-    }
-
-    // start NetworkTables
-    NetworkTableInstance ntinst = NetworkTableInstance.getDefault();
-    if (server) {
-      System.out.println("Setting up NetworkTables server");
-      ntinst.startServer();
-    } else {
-      System.out.println("Setting up NetworkTables client for team " + team);
-      ntinst.startClientTeam(team);
-    }
-
-    // start cameras
-    for (CameraConfig camera : cameras) {
-      startCamera(camera);
-    }
-
-    // loop forever
-    for (;;) {
-      try {
-        Thread.sleep(10000);
-      } catch (InterruptedException ex) {
-        return;
-      }
-    }
-  }
-}

cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java

diff --git a/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java b/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
new file mode 100644
index 0000000..70098cd
--- /dev/null
+++ b/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
@@ -0,0 +1,213 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi;
+
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
+
+import com.google.gson.Gson;
+import com.google.gson.GsonBuilder;
+import com.google.gson.JsonArray;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParser;
+
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/*
+   JSON format:
+   {
+       "team": <team number>,
+       "ntmode": <"client" or "server", "client" if unspecified>
+       "cameras": [
+           {
+               "name": <camera name>
+               "path": <path, e.g. "/dev/video0">
+               "pixel format": <"MJPEG", "YUYV", etc>   // optional
+               "width": <video mode width>              // optional
+               "height": <video mode height>            // optional
+               "fps": <video mode fps>                  // optional
+               "brightness": <percentage brightness>    // optional
+               "white balance": <"auto", "hold", value> // optional
+               "exposure": <"auto", "hold", value>      // optional
+               "properties": [                          // optional
+                   {
+                       "name": <property name>
+                       "value": <property value>
+                   }
+               ]
+           }
+       ]
+   }
+ */
+
+public final class Main {
+  private static String configFile = "/boot/frc.json";
+
+  @SuppressWarnings("MemberName")
+  public static class CameraConfig {
+    public String name;
+    public String path;
+    public JsonObject config;
+  }
+
+  public static int team;
+  public static boolean server;
+  public static List<CameraConfig> cameras = new ArrayList<>();
+
+  private Main() {
+  }
+
+  /**
+   * Report parse error.
+   */
+  public static void parseError(String str) {
+    System.err.println("config error in '" + configFile + "': " + str);
+  }
+
+  /**
+   * Read single camera configuration.
+   */
+  public static boolean readCameraConfig(JsonObject config) {
+    CameraConfig cam = new CameraConfig();
+
+    // name
+    JsonElement nameElement = config.get("name");
+    if (nameElement == null) {
+      parseError("could not read camera name");
+      return false;
+    }
+    cam.name = nameElement.getAsString();
+
+    // path
+    JsonElement pathElement = config.get("path");
+    if (pathElement == null) {
+      parseError("camera '" + cam.name + "': could not read path");
+      return false;
+    }
+    cam.path = pathElement.getAsString();
+
+    cam.config = config;
+
+    cameras.add(cam);
+    return true;
+  }
+
+  /**
+   * Read configuration file.
+   */
+  @SuppressWarnings("PMD.CyclomaticComplexity")
+  public static boolean readConfig() {
+    // parse file
+    JsonElement top;
+    try {
+      top = new JsonParser().parse(Files.newBufferedReader(Paths.get(configFile)));
+    } catch (IOException ex) {
+      System.err.println("could not open '" + configFile + "': " + ex);
+      return false;
+    }
+
+    // top level must be an object
+    if (!top.isJsonObject()) {
+      parseError("must be JSON object");
+      return false;
+    }
+    JsonObject obj = top.getAsJsonObject();
+
+    // team number
+    JsonElement teamElement = obj.get("team");
+    if (teamElement == null) {
+      parseError("could not read team number");
+      return false;
+    }
+    team = teamElement.getAsInt();
+
+    // ntmode (optional)
+    if (obj.has("ntmode")) {
+      String str = obj.get("ntmode").getAsString();
+      if ("client".equalsIgnoreCase(str)) {
+        server = false;
+      } else if ("server".equalsIgnoreCase(str)) {
+        server = true;
+      } else {
+        parseError("could not understand ntmode value '" + str + "'");
+      }
+    }
+
+    // cameras
+    JsonElement camerasElement = obj.get("cameras");
+    if (camerasElement == null) {
+      parseError("could not read cameras");
+      return false;
+    }
+    JsonArray cameras = camerasElement.getAsJsonArray();
+    for (JsonElement camera : cameras) {
+      if (!readCameraConfig(camera.getAsJsonObject())) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  /**
+   * Start running the camera.
+   */
+  public static void startCamera(CameraConfig config) {
+    System.out.println("Starting camera '" + config.name + "' on " + config.path);
+    VideoSource camera = CameraServer.getInstance().startAutomaticCapture(
+        config.name, config.path);
+
+    Gson gson = new GsonBuilder().create();
+
+    camera.setConfigJson(gson.toJson(config.config));
+  }
+
+  /**
+   * Main.
+   */
+  public static void main(String... args) {
+    if (args.length > 0) {
+      configFile = args[0];
+    }
+
+    // read configuration
+    if (!readConfig()) {
+      return;
+    }
+
+    // start NetworkTables
+    NetworkTableInstance ntinst = NetworkTableInstance.getDefault();
+    if (server) {
+      System.out.println("Setting up NetworkTables server");
+      ntinst.startServer();
+    } else {
+      System.out.println("Setting up NetworkTables client for team " + team);
+      ntinst.startClientTeam(team);
+    }
+
+    // start cameras
+    for (CameraConfig camera : cameras) {
+      startCamera(camera);
+    }
+
+    // loop forever
+    for (;;) {
+      try {
+        Thread.sleep(10000);
+      } catch (InterruptedException ex) {
+        return;
+      }
+    }
+  }
+}

cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java

diff --git a/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java b/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
index 3ef23c3..787b944 100644
--- a/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
+++ b/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -37,7 +37,6 @@
  * Singleton class for creating and keeping camera servers.
  * Also publishes camera information to NetworkTables.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public final class CameraServer {
   public static final int kBasePort = 1181;
 

cmake/modules/LinkMacOSGUI.cmake

diff --git a/cmake/modules/LinkMacOSGUI.cmake b/cmake/modules/LinkMacOSGUI.cmake
new file mode 100644
index 0000000..f76554e
--- /dev/null
+++ b/cmake/modules/LinkMacOSGUI.cmake
@@ -0,0 +1,5 @@
+macro(wpilib_link_macos_gui target)
+    if (APPLE)
+        set_target_properties(${target} PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore")
+    endif()
+endmacro()

cmake/toolchains/arm-frc-gnueabi.toolchain.cmake

diff --git a/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake b/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake
index 8e6536a..c144e1d 100644
--- a/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake
+++ b/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake
@@ -1,4 +1,4 @@
 set(GCC_COMPILER_VERSION "" CACHE STRING "GCC Compiler version")
-set(GNU_MACHINE "arm-frc2020-linux-gnueabi" CACHE STRING "GNU compiler triple")
+set(GNU_MACHINE "arm-frc2021-linux-gnueabi" CACHE STRING "GNU compiler triple")
 set(SOFTFP yes)
 include("${CMAKE_CURRENT_LIST_DIR}/arm.toolchain.cmake")

cscore/.styleguide

diff --git a/cscore/.styleguide b/cscore/.styleguide
index f38c850..fada841 100644
--- a/cscore/.styleguide
+++ b/cscore/.styleguide
@@ -6,6 +6,7 @@
   (?<!_c)\.h$
   \.hpp$
   \.inc$
+  \.inl$
 }
 
 cppSrcFileInclude {

cscore/CMakeLists.txt

diff --git a/cscore/CMakeLists.txt b/cscore/CMakeLists.txt
index a87ab7d..98f6d7e 100644
--- a/cscore/CMakeLists.txt
+++ b/cscore/CMakeLists.txt
@@ -3,6 +3,7 @@
 include(SubDirList)
 include(CompileWarnings)
 include(AddTest)
+include(LinkMacOSGUI)
 
 find_package( OpenCV REQUIRED )
 
@@ -39,7 +40,7 @@
 install(TARGETS cscore EXPORT cscore DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cscore")
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
     set (cscore_config_dir ${wpilib_dest})
 else()
     set (cscore_config_dir share/cscore)
@@ -52,16 +53,29 @@
 SUBDIR_LIST(cscore_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
 foreach(example ${cscore_examples})
     file(GLOB cscore_example_src examples/${example}/*.cpp)
+    unset(add_libs)
+    if(${example} STREQUAL "usbviewer")
+        if(TARGET wpigui)
+            set(add_libs wpigui)
+        else()
+            unset(cscore_example_src)
+        endif()
+    endif()
     if(cscore_example_src)
         add_executable(cscore_${example} ${cscore_example_src})
         wpilib_target_warnings(cscore_${example})
-        target_link_libraries(cscore_${example} cscore)
+
+        if (${example} STREQUAL "usbviewer")
+            wpilib_link_macos_gui(cscore_${example})
+        endif()
+
+        target_link_libraries(cscore_${example} cscore ${add_libs})
         set_property(TARGET cscore_${example} PROPERTY FOLDER "examples")
     endif()
 endforeach()
 
 # Java bindings
-if (NOT WITHOUT_JAVA)
+if (WITH_JAVA)
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
@@ -69,7 +83,9 @@
 
     #find java files, copy them locally
 
-    set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
+    if("${OPENCV_JAVA_INSTALL_DIR}" STREQUAL "")
+        set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
+    endif()
 
     find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
     find_file(OPENCV_JNI_FILE NAMES libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.so

cscore/build.gradle

diff --git a/cscore/build.gradle b/cscore/build.gradle
index 9b84031..90e5c20 100644
--- a/cscore/build.gradle
+++ b/cscore/build.gradle
@@ -15,15 +15,60 @@
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
+model {
+    components {
+        cscoreJNICvStatic(JniNativeLibrarySpec) {
+            baseName = 'cscorejnicvstatic'
+
+            enableCheckTask true
+            javaCompileTasks << compileJava
+            jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio)
+            jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.raspbian)
+            jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.aarch64bionic)
+
+            sources {
+                cpp {
+                    source {
+                        srcDirs 'src/main/native/cpp'
+                        include '**/jni/*.cpp'
+                    }
+                    exportedHeaders {
+                        srcDir 'src/main/native/include'
+                        include '**/*.h'
+                    }
+
+                }
+            }
+            binaries.all {
+                if (it instanceof StaticLibraryBinarySpec) {
+                    it.buildable = false
+                    return
+                }
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+
+                if (it.targetPlatform.operatingSystem.linux) {
+                    it.linker.args '-Wl,--version-script=' + file('src/main/native/LinuxSymbolScript.txt')
+                } else if (it.targetPlatform.operatingSystem.macOsX) {
+                    it.linker.args '-exported_symbols_list'
+                    it.linker.args file('src/main/native/MacSymbolScript.txt').toString()
+                }
+            }
+        }
+    }
+}
+
+
 ext {
     sharedCvConfigs = [cscore    : [],
                        cscoreBase: [],
                        cscoreDev : [],
                        cscoreTest: [],
                        cscoreJNIShared: []]
-    staticCvConfigs = [cscoreJNI: []]
+    staticCvConfigs = [cscoreJNI: [],
+                       cscoreJNICvStatic: []]
     useJava = true
     useCpp = true
+    cvStaticBuild = true
     splitSetup = {
         if (it.targetPlatform.operatingSystem.isMacOsX()) {
             it.sources {
@@ -112,22 +157,63 @@
             symbols.removeIf({ !it.startsWith('CS_') })
         }
     }
+    cscoreJNICvStatic {
+        x86SymbolFilter = { symbols ->
+            symbols.removeIf({ !it.startsWith('CS_') })
+        }
+        x64SymbolFilter = { symbols ->
+            symbols.removeIf({ !it.startsWith('CS_') })
+        }
+    }
 }
 
 model {
     components {
         examplesMap.each { key, value ->
-            "${key}"(NativeExecutableSpec) {
-                targetBuildTypes 'debug'
-                binaries.all {
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                    lib library: 'cscore', linkage: 'shared'
+            if (key == "usbviewer") {
+                if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
+                    "${key}"(NativeExecutableSpec) {
+                        targetBuildTypes 'debug'
+                        binaries.all {
+                            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                            lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                            lib library: 'cscore', linkage: 'shared'
+                            nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                            if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                                it.buildable = false
+                                return
+                            }
+                            if (it.targetPlatform.operatingSystem.isWindows()) {
+                                it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+                            } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                                it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+                            } else {
+                                it.linker.args << '-lX11'
+                            }
+                        }
+                        sources {
+                            cpp {
+                                source {
+                                    srcDirs 'examples/' + "${key}"
+                                    include '**/*.cpp'
+                                }
+                            }
+                        }
+                    }
                 }
-                sources {
-                    cpp {
-                        source {
-                            srcDirs 'examples/' + "${key}"
-                            include '**/*.cpp'
+            } else {
+                "${key}"(NativeExecutableSpec) {
+                    targetBuildTypes 'debug'
+                    binaries.all {
+                        lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                        lib library: 'cscore', linkage: 'shared'
+                    }
+                    sources {
+                        cpp {
+                            source {
+                                srcDirs 'examples/' + "${key}"
+                                include '**/*.cpp'
+                            }
                         }
                     }
                 }

cscore/examples/usbviewer/usbviewer.cpp

diff --git a/cscore/examples/usbviewer/usbviewer.cpp b/cscore/examples/usbviewer/usbviewer.cpp
new file mode 100644
index 0000000..75f961a
--- /dev/null
+++ b/cscore/examples/usbviewer/usbviewer.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <atomic>
+#include <thread>
+#include <vector>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <wpi/raw_ostream.h>
+#include <wpi/spinlock.h>
+#include <wpigui.h>
+
+#include "cscore.h"
+#include "cscore_cv.h"
+
+namespace gui = wpi::gui;
+
+int main() {
+  std::atomic<cv::Mat*> latestFrame{nullptr};
+  std::vector<cv::Mat*> sharedFreeList;
+  wpi::spinlock sharedFreeListMutex;
+  std::vector<cv::Mat*> sourceFreeList;
+  std::atomic<bool> stopCamera{false};
+
+  cs::UsbCamera camera{"usbcam", 0};
+  camera.SetVideoMode(cs::VideoMode::kMJPEG, 640, 480, 30);
+  cs::CvSink cvsink{"cvsink"};
+  cvsink.SetSource(camera);
+
+  std::thread thr([&] {
+    cv::Mat frame;
+    while (!stopCamera) {
+      // get frame from camera
+      uint64_t time = cvsink.GrabFrame(frame);
+      if (time == 0) {
+        wpi::outs() << "error: " << cvsink.GetError() << '\n';
+        continue;
+      }
+
+      // get or create a mat, prefer sourceFreeList over sharedFreeList
+      cv::Mat* out;
+      if (!sourceFreeList.empty()) {
+        out = sourceFreeList.back();
+        sourceFreeList.pop_back();
+      } else {
+        {
+          std::scoped_lock lock(sharedFreeListMutex);
+          for (auto mat : sharedFreeList) sourceFreeList.emplace_back(mat);
+          sharedFreeList.clear();
+        }
+        if (!sourceFreeList.empty()) {
+          out = sourceFreeList.back();
+          sourceFreeList.pop_back();
+        } else {
+          out = new cv::Mat;
+        }
+      }
+
+      // convert to RGBA
+      cv::cvtColor(frame, *out, cv::COLOR_BGR2RGBA);
+
+      // make available
+      auto prev = latestFrame.exchange(out);
+
+      // put prev on free list
+      if (prev) sourceFreeList.emplace_back(prev);
+    }
+  });
+
+  gui::CreateContext();
+  gui::Initialize("Hello World", 1024, 768);
+  gui::Texture tex;
+  gui::AddEarlyExecute([&] {
+    auto frame = latestFrame.exchange(nullptr);
+    if (frame) {
+      // create or update texture
+      if (!tex || frame->cols != tex.GetWidth() ||
+          frame->rows != tex.GetHeight()) {
+        tex = gui::Texture(gui::kPixelRGBA, frame->cols, frame->rows,
+                           frame->data);
+      } else {
+        tex.Update(frame->data);
+      }
+      // put back on shared freelist
+      std::scoped_lock lock(sharedFreeListMutex);
+      sharedFreeList.emplace_back(frame);
+    }
+
+    ImGui::SetNextWindowSize(ImVec2(640, 480), ImGuiCond_FirstUseEver);
+    if (ImGui::Begin("Video")) {
+      // render to window (best fit)
+      if (tex && tex.GetWidth() != 0 && tex.GetHeight() != 0) {
+        auto drawList = ImGui::GetWindowDrawList();
+        ImVec2 windowPos = ImGui::GetWindowPos();
+        ImVec2 imageMin = ImGui::GetWindowContentRegionMin();
+        ImVec2 imageMax = ImGui::GetWindowContentRegionMax();
+        gui::MaxFit(&imageMin, &imageMax, tex.GetWidth(), tex.GetHeight());
+        drawList->AddImage(tex, windowPos + imageMin, windowPos + imageMax);
+      }
+    }
+    ImGui::End();
+  });
+  gui::Main();
+  stopCamera = true;
+  thr.join();
+}

cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java

diff --git a/cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java b/cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java
index ac18408..b8a548c 100644
--- a/cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java
+++ b/cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -120,6 +120,7 @@
   //
   // UsbCamera Source Functions
   //
+  public static native void setUsbCameraPath(int source, String path);
   public static native String getUsbCameraPath(int source);
   public static native UsbCameraInfo getUsbCameraInfo(int source);
 

cscore/src/main/java/edu/wpi/cscore/UsbCamera.java

diff --git a/cscore/src/main/java/edu/wpi/cscore/UsbCamera.java b/cscore/src/main/java/edu/wpi/cscore/UsbCamera.java
index fc80047..7526335 100644
--- a/cscore/src/main/java/edu/wpi/cscore/UsbCamera.java
+++ b/cscore/src/main/java/edu/wpi/cscore/UsbCamera.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -41,6 +41,13 @@
   }
 
   /**
+   * Change the path to the device.
+   */
+  void setPath(String path) {
+    CameraServerJNI.setUsbCameraPath(m_handle, path);
+  }
+
+  /**
    * Get the path to the device.
    */
   public String getPath() {

cscore/src/main/native/LinuxSymbolScript.txt

diff --git a/cscore/src/main/native/LinuxSymbolScript.txt b/cscore/src/main/native/LinuxSymbolScript.txt
new file mode 100644
index 0000000..5058080
--- /dev/null
+++ b/cscore/src/main/native/LinuxSymbolScript.txt
@@ -0,0 +1,4 @@
+cscorejnicvstatic {
+  global: CS_*; JNI_*; Java_*; # explicitly list symbols to be exported
+  local: *;         # hide everything else
+};

cscore/src/main/native/MacSymbolScript.txt

diff --git a/cscore/src/main/native/MacSymbolScript.txt b/cscore/src/main/native/MacSymbolScript.txt
new file mode 100644
index 0000000..ecfe349
--- /dev/null
+++ b/cscore/src/main/native/MacSymbolScript.txt
@@ -0,0 +1,3 @@
+_CS_*
+_JNI_*
+_Java_*

cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp

diff --git a/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp b/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp
index b974169..3ba1784 100644
--- a/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp
+++ b/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -62,21 +62,21 @@
                                            int maximum, int step,
                                            int defaultValue, int value) {
   std::scoped_lock lock(m_mutex);
-  int ndx = CreateOrUpdateProperty(name,
-                                   [=] {
-                                     return std::make_unique<PropertyImpl>(
-                                         name, kind, minimum, maximum, step,
-                                         defaultValue, value);
-                                   },
-                                   [&](PropertyImpl& prop) {
-                                     // update all but value
-                                     prop.propKind = kind;
-                                     prop.minimum = minimum;
-                                     prop.maximum = maximum;
-                                     prop.step = step;
-                                     prop.defaultValue = defaultValue;
-                                     value = prop.value;
-                                   });
+  int ndx = CreateOrUpdateProperty(
+      name,
+      [=] {
+        return std::make_unique<PropertyImpl>(name, kind, minimum, maximum,
+                                              step, defaultValue, value);
+      },
+      [&](PropertyImpl& prop) {
+        // update all but value
+        prop.propKind = kind;
+        prop.minimum = minimum;
+        prop.maximum = maximum;
+        prop.step = step;
+        prop.defaultValue = defaultValue;
+        value = prop.value;
+      });
   m_notifier.NotifySourceProperty(*this, CS_SOURCE_PROPERTY_CREATED, name, ndx,
                                   kind, value, wpi::Twine{});
   return ndx;

cscore/src/main/native/cpp/UsbCameraImplCommon.cpp

diff --git a/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp b/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp
index 2b3fb08..41906a7 100644
--- a/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp
+++ b/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -44,6 +44,11 @@
   return cs::CreateUsbCameraPath(name, path, status);
 }
 
+void CS_SetUsbCameraPath(CS_Source source, const char* path,
+                         CS_Status* status) {
+  cs::SetUsbCameraPath(source, path, status);
+}
+
 char* CS_GetUsbCameraPath(CS_Source source, CS_Status* status) {
   return ConvertToC(cs::GetUsbCameraPath(source, status));
 }

cscore/src/main/native/cpp/jni/CameraServerJNI.cpp

diff --git a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
index 52a0fea..e9408fd 100644
--- a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
+++ b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -1028,6 +1028,20 @@
 
 /*
  * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    setUsbCameraPath
+ * Signature: (ILjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_cscore_CameraServerJNI_setUsbCameraPath
+  (JNIEnv* env, jclass, jint source, jstring path)
+{
+  CS_Status status = 0;
+  cs::SetUsbCameraPath(source, JStringRef{env, path}.str(), &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
  * Method:    getUsbCameraPath
  * Signature: (I)Ljava/lang/String;
  */

cscore/src/main/native/include/cscore_c.h

diff --git a/cscore/src/main/native/include/cscore_c.h b/cscore/src/main/native/include/cscore_c.h
index 24e30b4..ee5d33a 100644
--- a/cscore/src/main/native/include/cscore_c.h
+++ b/cscore/src/main/native/include/cscore_c.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -228,7 +228,7 @@
 };
 
 /**
- * USB camera infomation
+ * USB camera information
  */
 typedef struct CS_UsbCameraInfo {
   int dev;
@@ -338,6 +338,7 @@
  * @defgroup cscore_usbcamera_cfunc UsbCamera Source Functions
  * @{
  */
+void CS_SetUsbCameraPath(CS_Source source, const char* path, CS_Status* status);
 char* CS_GetUsbCameraPath(CS_Source source, CS_Status* status);
 CS_UsbCameraInfo* CS_GetUsbCameraInfo(CS_Source source, CS_Status* status);
 /** @} */

cscore/src/main/native/include/cscore_cpp.h

diff --git a/cscore/src/main/native/include/cscore_cpp.h b/cscore/src/main/native/include/cscore_cpp.h
index c1ec024..1ee1615 100644
--- a/cscore/src/main/native/include/cscore_cpp.h
+++ b/cscore/src/main/native/include/cscore_cpp.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -274,6 +274,7 @@
  * @defgroup cscore_usbcamera_func UsbCamera Source Functions
  * @{
  */
+void SetUsbCameraPath(CS_Source, const wpi::Twine& path, CS_Status* status);
 std::string GetUsbCameraPath(CS_Source source, CS_Status* status);
 UsbCameraInfo GetUsbCameraInfo(CS_Source source, CS_Status* status);
 /** @} */

cscore/src/main/native/include/cscore_oo.h

diff --git a/cscore/src/main/native/include/cscore_oo.h b/cscore/src/main/native/include/cscore_oo.h
index 5ab0f70..9e6e290 100644
--- a/cscore/src/main/native/include/cscore_oo.h
+++ b/cscore/src/main/native/include/cscore_oo.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -445,6 +445,11 @@
   static std::vector<UsbCameraInfo> EnumerateUsbCameras();
 
   /**
+   * Change the path to the device.
+   */
+  void SetPath(const wpi::Twine& path);
+
+  /**
    * Get the path to the device.
    */
   std::string GetPath() const;

cscore/src/main/native/include/cscore_oo.inl

diff --git a/cscore/src/main/native/include/cscore_oo.inl b/cscore/src/main/native/include/cscore_oo.inl
index ffd2b23..2d56e1c 100644
--- a/cscore/src/main/native/include/cscore_oo.inl
+++ b/cscore/src/main/native/include/cscore_oo.inl
@@ -1,12 +1,16 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2015. All Rights Reserved.                             */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#ifndef CSCORE_OO_INL_
-#define CSCORE_OO_INL_
+#ifndef CSCORE_CSCORE_OO_INL_
+#define CSCORE_CSCORE_OO_INL_
+
+#include <string>
+#include <utility>
+#include <vector>
 
 namespace cs {
 
@@ -76,7 +80,7 @@
 }
 
 inline VideoProperty::VideoProperty(CS_Property handle, Kind kind)
-  : m_status(0), m_handle(handle), m_kind(kind) {}
+    : m_status(0), m_handle(handle), m_kind(kind) {}
 
 inline VideoSource::VideoSource(const VideoSource& source)
     : m_handle(source.m_handle == 0 ? 0
@@ -255,6 +259,11 @@
   return ::cs::EnumerateUsbCameras(&status);
 }
 
+inline void UsbCamera::SetPath(const wpi::Twine& path) {
+  m_status = 0;
+  return ::cs::SetUsbCameraPath(m_handle, path, &m_status);
+}
+
 inline std::string UsbCamera::GetPath() const {
   m_status = 0;
   return ::cs::GetUsbCameraPath(m_handle, &m_status);
@@ -394,10 +403,10 @@
 }
 
 inline VideoProperty ImageSource::CreateProperty(const wpi::Twine& name,
-                                              VideoProperty::Kind kind,
-                                              int minimum, int maximum,
-                                              int step, int defaultValue,
-                                              int value) {
+                                                 VideoProperty::Kind kind,
+                                                 int minimum, int maximum,
+                                                 int step, int defaultValue,
+                                                 int value) {
   m_status = 0;
   return VideoProperty{CreateSourceProperty(
       m_handle, name, static_cast<CS_PropertyKind>(static_cast<int>(kind)),
@@ -405,35 +414,41 @@
 }
 
 inline VideoProperty ImageSource::CreateIntegerProperty(const wpi::Twine& name,
-                                                    int minimum, int maximum,
-                                                    int step, int defaultValue,
-                                                    int value) {
+                                                        int minimum,
+                                                        int maximum, int step,
+                                                        int defaultValue,
+                                                        int value) {
   m_status = 0;
   return VideoProperty{CreateSourceProperty(
-      m_handle, name, static_cast<CS_PropertyKind>(static_cast<int>(VideoProperty::Kind::kInteger)),
+      m_handle, name,
+      static_cast<CS_PropertyKind>(
+          static_cast<int>(VideoProperty::Kind::kInteger)),
       minimum, maximum, step, defaultValue, value, &m_status)};
 }
 
 inline VideoProperty ImageSource::CreateBooleanProperty(const wpi::Twine& name,
-                                                     bool defaultValue,
-                                                     bool value) {
+                                                        bool defaultValue,
+                                                        bool value) {
   m_status = 0;
   return VideoProperty{CreateSourceProperty(
-      m_handle, name, static_cast<CS_PropertyKind>(static_cast<int>(VideoProperty::Kind::kBoolean)),
+      m_handle, name,
+      static_cast<CS_PropertyKind>(
+          static_cast<int>(VideoProperty::Kind::kBoolean)),
       0, 1, 1, defaultValue ? 1 : 0, value ? 1 : 0, &m_status)};
 }
 
-inline VideoProperty ImageSource::CreateStringProperty(const wpi::Twine& name,
-                                                    const wpi::Twine& value) {
+inline VideoProperty ImageSource::CreateStringProperty(
+    const wpi::Twine& name, const wpi::Twine& value) {
   m_status = 0;
-  auto prop = VideoProperty{CreateSourceProperty(
-      m_handle, name, static_cast<CS_PropertyKind>(static_cast<int>(VideoProperty::Kind::kString)),
-      0, 0, 0, 0, 0, &m_status)};
+  auto prop = VideoProperty{
+      CreateSourceProperty(m_handle, name,
+                           static_cast<CS_PropertyKind>(
+                               static_cast<int>(VideoProperty::Kind::kString)),
+                           0, 0, 0, 0, 0, &m_status)};
   prop.SetString(value);
   return prop;
 }
 
-
 inline void ImageSource::SetEnumPropertyChoices(
     const VideoProperty& property, wpi::ArrayRef<std::string> choices) {
   m_status = 0;
@@ -441,8 +456,8 @@
 }
 
 template <typename T>
-inline void ImageSource::SetEnumPropertyChoices(const VideoProperty& property,
-                                             std::initializer_list<T> choices) {
+inline void ImageSource::SetEnumPropertyChoices(
+    const VideoProperty& property, std::initializer_list<T> choices) {
   std::vector<std::string> vec;
   vec.reserve(choices.size());
   for (const auto& choice : choices) vec.emplace_back(choice);
@@ -618,4 +633,4 @@
 
 }  // namespace cs
 
-#endif  /* CSCORE_OO_INL_ */
+#endif  // CSCORE_CSCORE_OO_INL_

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

diff --git a/cscore/src/main/native/linux/UsbCameraImpl.cpp b/cscore/src/main/native/linux/UsbCameraImpl.cpp
index a75d3cd..1175ca3 100644
--- a/cscore/src/main/native/linux/UsbCameraImpl.cpp
+++ b/cscore/src/main/native/linux/UsbCameraImpl.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -102,7 +102,7 @@
   if (name.startswith("raw_")) name = name.substr(4);
   return name == "brightness" || name == "contrast" || name == "saturation" ||
          name == "hue" || name == "sharpness" || name == "gain" ||
-         name == "exposure_absolute";
+         name == "exposure_absolute" || name == "exposure_time_absolute";
 }
 
 static constexpr const int quirkLifeCamHd3000[] = {
@@ -112,6 +112,11 @@
 static constexpr char const* quirkPS3EyePropExValue = "exposure";
 static constexpr const int quirkPS3EyePropExAutoOn = 0;
 static constexpr const int quirkPS3EyePropExAutoOff = 1;
+static constexpr char const* quirkPiCameraPropExAuto = "auto_exposure";
+static constexpr char const* quirkPiCameraPropExValue =
+    "exposure_time_absolute";
+static constexpr const int quirkPiCameraPropExAutoOn = 0;
+static constexpr const int quirkPiCameraPropExAutoOff = 1;
 
 int UsbCameraImpl::RawToPercentage(const UsbCameraProperty& rawProp,
                                    int rawValue) {
@@ -218,6 +223,25 @@
   return true;
 }
 
+static bool IsVideoCaptureDevice(const char* cpath) {
+  int fd = open(cpath, O_RDWR);
+  if (fd < 0) return false;
+
+  struct v4l2_capability vcap;
+  std::memset(&vcap, 0, sizeof(vcap));
+  if (DoIoctl(fd, VIDIOC_QUERYCAP, &vcap) < 0) {
+    close(fd);
+    return false;
+  }
+  close(fd);
+
+  return (vcap.capabilities & V4L2_CAP_VIDEO_CAPTURE) != 0 &&
+         (vcap.capabilities & V4L2_CAP_STREAMING) != 0 &&
+         ((vcap.capabilities & V4L2_CAP_DEVICE_CAPS) == 0 ||
+          ((vcap.device_caps & V4L2_CAP_VIDEO_CAPTURE) != 0 &&
+           (vcap.device_caps & V4L2_CAP_STREAMING) != 0));
+}
+
 static int GetDeviceNum(const char* cpath) {
   wpi::StringRef path{cpath};
   std::string pathBuf;
@@ -259,10 +283,10 @@
                              Notifier& notifier, Telemetry& telemetry,
                              const wpi::Twine& path)
     : SourceImpl{name, logger, notifier, telemetry},
-      m_path{path.str()},
       m_fd{-1},
       m_command_fd{eventfd(0, 0)},
-      m_active{true} {
+      m_active{true},
+      m_path{path.str()} {
   SetDescription(GetDescriptionImpl(m_path.c_str()));
   SetQuirks();
 
@@ -760,6 +784,22 @@
   return CS_OK;
 }
 
+CS_StatusValue UsbCameraImpl::DeviceCmdSetPath(
+    std::unique_lock<wpi::mutex>& lock, const Message& msg) {
+  m_path = msg.dataStr;
+  lock.unlock();
+  // disconnect and reconnect
+  bool wasStreaming = m_streaming;
+  if (wasStreaming) DeviceStreamOff();
+  if (m_fd >= 0) {
+    DeviceDisconnect();
+    DeviceConnect();
+  }
+  if (wasStreaming) DeviceStreamOn();
+  lock.lock();
+  return CS_OK;
+}
+
 CS_StatusValue UsbCameraImpl::DeviceProcessCommand(
     std::unique_lock<wpi::mutex>& lock, const Message& msg) {
   if (msg.kind == Message::kCmdSetMode ||
@@ -773,6 +813,8 @@
   } else if (msg.kind == Message::kNumSinksChanged ||
              msg.kind == Message::kNumSinksEnabledChanged) {
     return CS_OK;
+  } else if (msg.kind == Message::kCmdSetPath) {
+    return DeviceCmdSetPath(lock, msg);
   } else {
     return CS_OK;
   }
@@ -1112,6 +1154,25 @@
     }
   }
 
+  // The Pi camera reports mode ranges, which we don't currently handle, so only
+  // provide a set of discrete modes; list based on
+  // https://picamera.readthedocs.io/en/release-1.10/fov.html
+  if (modes.empty() && m_picamera) {
+    for (VideoMode::PixelFormat pixelFormat :
+         {VideoMode::kYUYV, VideoMode::kMJPEG, VideoMode::kBGR}) {
+      modes.emplace_back(pixelFormat, 1920, 1080, 30);
+      modes.emplace_back(pixelFormat, 2592, 1944, 15);
+      modes.emplace_back(pixelFormat, 1296, 972, 42);
+      modes.emplace_back(pixelFormat, 1296, 730, 49);
+      modes.emplace_back(pixelFormat, 640, 480, 90);
+      modes.emplace_back(pixelFormat, 320, 240, 90);
+      modes.emplace_back(pixelFormat, 160, 120, 90);
+      modes.emplace_back(pixelFormat, 640, 480, 60);
+      modes.emplace_back(pixelFormat, 320, 240, 60);
+      modes.emplace_back(pixelFormat, 160, 120, 60);
+    }
+  }
+
   {
     std::scoped_lock lock(m_mutex);
     m_videoModes.swap(modes);
@@ -1192,6 +1253,7 @@
   wpi::StringRef desc = GetDescription(descbuf);
   m_lifecam_exposure =
       desc.endswith("LifeCam HD-3000") || desc.endswith("LifeCam Cinema (TM)");
+  m_picamera = desc.startswith("mmal service");
 
   int deviceNum = GetDeviceNum(m_path.c_str());
   if (deviceNum >= 0) {
@@ -1248,7 +1310,9 @@
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOn, status);
-
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOn, status);
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 3, status);
   }
@@ -1258,6 +1322,9 @@
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOff, status);  // manual
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOff, status);  // manual
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 1, status);  // manual
   }
@@ -1267,6 +1334,9 @@
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExAuto),
                 quirkPS3EyePropExAutoOff, status);  // manual
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExAuto),
+                quirkPiCameraPropExAutoOff, status);  // manual
   } else {
     SetProperty(GetPropertyIndex(kPropExAuto), 1, status);  // manual
   }
@@ -1277,6 +1347,8 @@
   }
   if (m_ps3eyecam_exposure) {
     SetProperty(GetPropertyIndex(quirkPS3EyePropExValue), value, status);
+  } else if (m_picamera) {
+    SetProperty(GetPropertyIndex(quirkPiCameraPropExValue), value, status);
   } else {
     SetProperty(GetPropertyIndex(kPropExValue), value, status);
   }
@@ -1323,6 +1395,17 @@
   Send(Message{Message::kNumSinksEnabledChanged});
 }
 
+void UsbCameraImpl::SetPath(const wpi::Twine& path, CS_Status* status) {
+  Message msg{Message::kCmdSetPath};
+  msg.dataStr = path.str();
+  *status = SendAndWait(std::move(msg));
+}
+
+std::string UsbCameraImpl::GetPath() const {
+  std::scoped_lock lock(m_mutex);
+  return m_path;
+}
+
 namespace cs {
 
 CS_Source CreateUsbCameraDev(const wpi::Twine& name, int dev,
@@ -1341,6 +1424,16 @@
                                               inst.telemetry, path));
 }
 
+void SetUsbCameraPath(CS_Source source, const wpi::Twine& path,
+                      CS_Status* status) {
+  auto data = Instance::GetInstance().GetSource(source);
+  if (!data || data->kind != CS_SOURCE_USB) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  static_cast<UsbCameraImpl&>(*data->source).SetPath(path, status);
+}
+
 std::string GetUsbCameraPath(CS_Source source, CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || data->kind != CS_SOURCE_USB) {
@@ -1418,6 +1511,8 @@
       path += fname;
       info.path = path.str();
 
+      if (!IsVideoCaptureDevice(path.c_str())) continue;
+
       info.name = GetDescriptionImpl(path.c_str());
       if (info.name.empty()) continue;
 

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

diff --git a/cscore/src/main/native/linux/UsbCameraImpl.h b/cscore/src/main/native/linux/UsbCameraImpl.h
index 3627228..62b94a0 100644
--- a/cscore/src/main/native/linux/UsbCameraImpl.h
+++ b/cscore/src/main/native/linux/UsbCameraImpl.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -66,12 +66,14 @@
   void NumSinksChanged() override;
   void NumSinksEnabledChanged() override;
 
-  std::string GetPath() { return m_path; }
+  void SetPath(const wpi::Twine& path, CS_Status* status);
+  std::string GetPath() const;
 
   // Messages passed to/from camera thread
   struct Message {
     enum Kind {
       kNone = 0,
+      kCmdSetPath,
       kCmdSetMode,
       kCmdSetPixelFormat,
       kCmdSetResolution,
@@ -132,6 +134,8 @@
                                   const Message& msg);
   CS_StatusValue DeviceCmdSetProperty(std::unique_lock<wpi::mutex>& lock,
                                       const Message& msg);
+  CS_StatusValue DeviceCmdSetPath(std::unique_lock<wpi::mutex>& lock,
+                                  const Message& msg);
 
   // Property helper functions
   int RawToPercentage(const UsbCameraProperty& rawProp, int rawValue);
@@ -152,11 +156,6 @@
   static constexpr int kNumBuffers = 4;
   std::array<UsbCameraBuffer, kNumBuffers> m_buffers;
 
-  //
-  // Path never changes, so not protected by mutex.
-  //
-  std::string m_path;
-
   std::atomic_int m_fd;
   std::atomic_int m_command_fd;  // for command eventfd
 
@@ -166,6 +165,7 @@
   // Quirks
   bool m_lifecam_exposure{false};    // Microsoft LifeCam exposure
   bool m_ps3eyecam_exposure{false};  // PS3 Eyecam exposure
+  bool m_picamera{false};            // Raspberry Pi camera
 
   //
   // Variables protected by m_mutex
@@ -175,6 +175,9 @@
   mutable std::vector<Message> m_commands;
   mutable std::vector<std::pair<std::thread::id, CS_StatusValue>> m_responses;
   mutable wpi::condition_variable m_responseCv;
+
+  // Path
+  std::string m_path;
 };
 
 }  // namespace cs

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

diff --git a/cscore/src/main/native/osx/UsbCameraImpl.cpp b/cscore/src/main/native/osx/UsbCameraImpl.cpp
index a10fae4..abefcc7 100644
--- a/cscore/src/main/native/osx/UsbCameraImpl.cpp
+++ b/cscore/src/main/native/osx/UsbCameraImpl.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,11 @@
   return 0;
 }
 
+void SetUsbCameraPath(CS_Source source, const wpi::Twine& path,
+                      CS_Status* status) {
+  *status = CS_INVALID_HANDLE;
+}
+
 std::string GetUsbCameraPath(CS_Source source, CS_Status* status) {
   *status = CS_INVALID_HANDLE;
   return std::string{};

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

diff --git a/cscore/src/main/native/windows/UsbCameraImpl.cpp b/cscore/src/main/native/windows/UsbCameraImpl.cpp
index 69c1fcb..784f5ad 100644
--- a/cscore/src/main/native/windows/UsbCameraImpl.cpp
+++ b/cscore/src/main/native/windows/UsbCameraImpl.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -198,6 +198,20 @@
       SetCameraMessage, Message::kNumSinksEnabledChanged, nullptr);
 }
 
+void UsbCameraImpl::SetPath(const wpi::Twine& path, CS_Status* status) {
+  Message msg{Message::kCmdSetPath};
+  msg.dataStr = path.str();
+  auto result =
+      m_messagePump->SendWindowMessage<CS_Status, Message::Kind, Message*>(
+          SetCameraMessage, msg.kind, &msg);
+  *status = result;
+}
+
+std::string UsbCameraImpl::GetPath() const {
+  std::scoped_lock lock(m_mutex);
+  return m_path;
+}
+
 void UsbCameraImpl::StartMessagePump() {
   m_messagePump = std::make_unique<WindowsMessagePump>(
       [this](HWND hwnd, UINT uiMsg, WPARAM wParam, LPARAM lParam) {
@@ -705,6 +719,16 @@
       DeviceStreamOn();
     }
     return CS_OK;
+  } else if (msgKind == Message::kCmdSetPath) {
+    {
+      std::scoped_lock lock(m_mutex);
+      m_path = msg->dataStr;
+      std::wstring_convert<std::codecvt_utf8<wchar_t>> utf8_conv;
+      m_widePath = utf8_conv.from_bytes(m_path.c_str());
+    }
+    DeviceDisconnect();
+    DeviceConnect();
+    return CS_OK;
   } else {
     return CS_OK;
   }
@@ -966,6 +990,27 @@
   m_notifier.NotifySource(*this, CS_SOURCE_VIDEOMODES_UPDATED);
 }
 
+static void ParseVidAndPid(wpi::StringRef path, int* pid, int* vid) {
+  auto vidIndex = path.find_lower("vid_");
+  auto pidIndex = path.find_lower("pid_");
+
+  if (vidIndex != wpi::StringRef::npos) {
+    auto vidSlice = path.slice(vidIndex + 4, vidIndex + 8);
+    uint16_t val = 0;
+    if (!vidSlice.getAsInteger(16, val)) {
+      *vid = val;
+    }
+  }
+
+  if (pidIndex != wpi::StringRef::npos) {
+    auto pidSlice = path.slice(pidIndex + 4, pidIndex + 8);
+    uint16_t val = 0;
+    if (!pidSlice.getAsInteger(16, val)) {
+      *pid = val;
+    }
+  }
+}
+
 std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status) {
   std::vector<UsbCameraInfo> retval;
 
@@ -1012,6 +1057,10 @@
         MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_SYMBOLIC_LINK, buf,
         sizeof(buf) / sizeof(WCHAR), NULL);
     info.path = utf8_conv.to_bytes(buf);
+
+    // Try to parse path from symbolic link
+    ParseVidAndPid(info.path, &info.productId, &info.vendorId);
+
     retval.emplace_back(std::move(info));
   }
 
@@ -1052,6 +1101,16 @@
   return inst.CreateSource(CS_SOURCE_USB, source);
 }
 
+void SetUsbCameraPath(CS_Source source, const wpi::Twine& path,
+                      CS_Status* status) {
+  auto data = Instance::GetInstance().GetSource(source);
+  if (!data || data->kind != CS_SOURCE_USB) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  static_cast<UsbCameraImpl&>(*data->source).SetPath(path, status);
+}
+
 std::string GetUsbCameraPath(CS_Source source, CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || data->kind != CS_SOURCE_USB) {
@@ -1070,7 +1129,10 @@
   }
 
   info.path = static_cast<UsbCameraImpl&>(*data->source).GetPath();
-  // TODO: dev and name
+  wpi::SmallVector<char, 64> buf;
+  info.name = static_cast<UsbCameraImpl&>(*data->source).GetDescription(buf);
+  ParseVidAndPid(info.path, &info.productId, &info.vendorId);
+  info.dev = -1;  // We have lost dev information by this point in time.
   return info;
 }
 

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

diff --git a/cscore/src/main/native/windows/UsbCameraImpl.h b/cscore/src/main/native/windows/UsbCameraImpl.h
index 11e07aa..60a3080 100644
--- a/cscore/src/main/native/windows/UsbCameraImpl.h
+++ b/cscore/src/main/native/windows/UsbCameraImpl.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -78,12 +78,14 @@
   void ProcessFrame(IMFSample* sample, const VideoMode& mode);
   void PostRequestNewFrame();
 
-  std::string GetPath() { return m_path; }
+  void SetPath(const wpi::Twine& path, CS_Status* status);
+  std::string GetPath() const;
 
   // Messages passed to/from camera thread
   struct Message {
     enum Kind {
       kNone = 0,
+      kCmdSetPath,
       kCmdSetMode,
       kCmdSetPixelFormat,
       kCmdSetResolution,
@@ -116,9 +118,6 @@
   bool CacheProperties(CS_Status* status) const override;
 
  private:
-  // The camera processing thread
-  void CameraThreadMain();
-
   LRESULT PumpMain(HWND hwnd, UINT uiMsg, WPARAM wParam, LPARAM lParam);
 
   bool CheckDeviceChange(WPARAM wParam, DEV_BROADCAST_HDR* pHdr,
@@ -171,9 +170,6 @@
   std::unique_ptr<cs::WindowsMessagePump> m_messagePump;
   ComPtr<IMFMediaType> m_currentMode;
 
-  //
-  // Path never changes, so not protected by mutex.
-  //
   std::string m_path;
 
   std::wstring m_widePath;

cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java

diff --git a/cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java b/cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java
index 9df3645..bc7866d 100644
--- a/cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java
+++ b/cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -56,6 +56,6 @@
   private static int getNonexistentCameraDev() {
     return Arrays.stream(CameraServerJNI.enumerateUsbCameras())
         .mapToInt(info -> info.dev)
-        .max().orElse(-1) + 1;
+        .max().orElse(-1) + 20;
   }
 }

docs/build.gradle

diff --git a/docs/build.gradle b/docs/build.gradle
index 10ba02b..6de7f84 100644
--- a/docs/build.gradle
+++ b/docs/build.gradle
@@ -8,6 +8,7 @@
 evaluationDependsOn(':cscore')
 evaluationDependsOn(':hal')
 evaluationDependsOn(':cameraserver')
+evaluationDependsOn(':wpimath')
 evaluationDependsOn(':wpilibc')
 evaluationDependsOn(':wpilibj')
 evaluationDependsOn(':wpilibOldCommands')
@@ -30,14 +31,16 @@
 cppProjectZips.add(project(':ntcore').cppHeadersZip)
 cppProjectZips.add(project(':cscore').cppHeadersZip)
 cppProjectZips.add(project(':cameraserver').cppHeadersZip)
+cppProjectZips.add(project(':wpimath').cppHeadersZip)
 cppProjectZips.add(project(':wpilibc').cppHeadersZip)
 cppProjectZips.add(project(':wpilibOldCommands').cppHeadersZip)
 cppProjectZips.add(project(':wpilibNewCommands').cppHeadersZip)
 
 doxygen {
-  executables {
-     doxygen version : '1.8.16'
-  }
+    executables {
+        doxygen version : '1.8.18',
+            baseURI : 'https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen'
+    }
 }
 
 doxygen {
@@ -101,17 +104,19 @@
 apply from: "${rootDir}/shared/opencv.gradle"
 
 task generateJavaDocs(type: Javadoc) {
-    classpath += project(":wpiutil").sourceSets.main.compileClasspath
+    classpath += project(":wpimath").sourceSets.main.compileClasspath
     options.links("https://docs.oracle.com/en/java/javase/11/docs/api/")
     options.addStringOption "tag", "pre:a:Pre-Condition"
     options.addBooleanOption "Xdoclint:html,missing,reference,syntax", true
     options.addBooleanOption('html5', true)
     dependsOn project(':wpilibj').generateJavaVersion
     dependsOn project(':hal').generateUsageReporting
+    dependsOn project(':wpimath').generateNat
     source project(':hal').sourceSets.main.java
     source project(':wpiutil').sourceSets.main.java
     source project(':cscore').sourceSets.main.java
     source project(':ntcore').sourceSets.main.java
+    source project(':wpimath').sourceSets.main.java
     source project(':wpilibj').sourceSets.main.java
     source project(':cameraserver').sourceSets.main.java
     source project(':wpilibOldCommands').sourceSets.main.java
@@ -125,6 +130,9 @@
     ext.entryPoint = "$destinationDir/index.html"
 
     if (JavaVersion.current().isJava11Compatible()) {
+        if (!JavaVersion.current().isJava12Compatible()) {
+            options.addBooleanOption('-no-module-directories', true)
+        }
         doLast {
             // This is a work-around for https://bugs.openjdk.java.net/browse/JDK-8211194. Can be removed once that issue is fixed on JDK's side
             // Since JDK 11, package-list is missing from javadoc output files and superseded by element-list file, but a lot of external tools still need it
@@ -142,11 +150,10 @@
     into '/'
 }
 
-addTaskToCopyAllOutputs(zipCppDocs)
-addTaskToCopyAllOutputs(zipJavaDocs)
-
-build.dependsOn zipCppDocs
-build.dependsOn zipJavaDocs
+tasks.register("zipDocs") {
+    dependsOn zipCppDocs
+    dependsOn zipJavaDocs
+}
 
 apply plugin: 'maven-publish'
 

generate_FRCUsageReporting.py

diff --git a/generate_FRCUsageReporting.py b/generate_FRCUsageReporting.py
deleted file mode 100644
index 23b9807..0000000
--- a/generate_FRCUsageReporting.py
+++ /dev/null
@@ -1,25 +0,0 @@
-from __future__ import print_function
-
-import sys
-
-header_in_name = sys.argv[1]
-instances_txt_name = sys.argv[2]
-resource_type_txt_name = sys.argv[3]
-header_out_name = sys.argv[4]
-
-with open(header_in_name, 'r') as f:
-  header_in = f.read().replace('\r', '')
-
-with open(instances_txt_name, 'r') as f:
-  instances_txt = [l.strip() for l in f.readlines()]
-
-with open(resource_type_txt_name, 'r') as f:
-  resource_type_txt = [l.strip() for l in f.readlines()]
-
-with open(header_out_name, 'w') as out:
-  header = header_in
-  header = header.replace('${usage_reporting_types_cpp}',
-                          ',\n'.join(resource_type_txt))
-  header = header.replace('${usage_reporting_instances_cpp}',
-                          ',\n'.join(instances_txt))
-  out.write(header)

gradle.properties

diff --git a/gradle.properties b/gradle.properties
new file mode 100644
index 0000000..fad0c09
--- /dev/null
+++ b/gradle.properties
@@ -0,0 +1 @@
+org.gradle.jvmargs=-Xmx1g

gradle/wrapper/gradle-wrapper.properties

diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
index 6ce793f..9492014 100644
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,5 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.0-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists

hal/CMakeLists.txt

diff --git a/hal/CMakeLists.txt b/hal/CMakeLists.txt
index c29f770..2e99064 100644
--- a/hal/CMakeLists.txt
+++ b/hal/CMakeLists.txt
@@ -37,7 +37,7 @@
 if(USE_EXTERNAL_HAL)
     include(${EXTERNAL_HAL_FILE})
 else()
-    target_sources(hal PRIVATE ${hal_sim_native_src} ${hal_sim_jni_src})
+    target_sources(hal PRIVATE ${hal_sim_native_src})
 endif()
 
 configure_file(src/generate/FRCUsageReporting.h.in gen/hal/FRCUsageReporting.h)
@@ -57,9 +57,9 @@
 
 install(TARGETS hal EXPORT hal DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/hal")
-install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/gen DESTINATION "${include_dest}/hal")
+install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/gen/ DESTINATION "${include_dest}/hal")
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
     set (hal_config_dir ${wpilib_dest})
 else()
     set (hal_config_dir share/hal)
@@ -70,7 +70,7 @@
 install(EXPORT hal DESTINATION ${hal_config_dir})
 
 # Java bindings
-if (NOT WITHOUT_JAVA)
+if (WITH_JAVA)
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
@@ -78,9 +78,7 @@
 
     configure_file(src/generate/FRCNetComm.java.in FRCNetComm.java)
 
-    file(GLOB
-        hal_shared_jni_src src/main/native/cpp/jni/*.cpp
-        hal_sim_jni_src src/main/native/sim/jni/*.cpp)
+    file(GLOB_RECURSE hal_shared_jni_src src/main/native/cpp/jni/*.cpp)
 
     file(GLOB_RECURSE JAVA_SOURCES
         ${CMAKE_CURRENT_BINARY_DIR}/FRCNetComm.java
@@ -103,8 +101,6 @@
 
     if(USE_EXTERNAL_HAL)
         include(${EXTERNAL_HAL_FILE})
-    else()
-        target_sources(haljni PRIVATE ${hal_sim_jni_src})
     endif()
 
     set_target_properties(haljni PROPERTIES OUTPUT_NAME "wpiHaljni")

hal/build.gradle

diff --git a/hal/build.gradle b/hal/build.gradle
index d744259..e5b5f6c 100644
--- a/hal/build.gradle
+++ b/hal/build.gradle
@@ -62,40 +62,13 @@
 
     nativeName = 'hal'
     setBaseName = 'wpiHal'
-    devMain = 'DevMain'
+    devMain = 'edu.wpi.first.hal.DevMain'
     niLibraries = true
     generatedHeaders = "$buildDir/generated/headers"
     jniSplitSetup = {
         it.tasks.withType(AbstractNativeSourceCompileTask) {
             it.dependsOn generateUsageReporting
         }
-        if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-            it.sources {
-                athenaJniCpp(CppSourceSet) {
-                    source {
-                        srcDirs = ["${rootDir}/shared/singlelib", "$buildDir/generated/cpp"]
-                        include '**/*.cpp'
-                    }
-                    exportedHeaders {
-                        srcDir 'src/main/native/include'
-                        srcDir generatedHeaders
-                    }
-                }
-            }
-        } else {
-            it.sources {
-                simJniCpp(CppSourceSet) {
-                    source {
-                        srcDirs 'src/main/native/sim'
-                        include '**/jni/*.cpp'
-                    }
-                    exportedHeaders {
-                        srcDir 'src/main/native/include'
-                        srcDir generatedHeaders
-                    }
-                }
-            }
-        }
     }
     splitSetup = {
         it.tasks.withType(AbstractNativeSourceCompileTask) {
@@ -107,7 +80,6 @@
                     source {
                         srcDirs = ['src/main/native/athena']
                         include '**/*.cpp'
-                        exclude '**/jni/*.cpp'
                     }
                     exportedHeaders {
                         srcDir 'src/main/native/include'
@@ -121,7 +93,6 @@
                     source {
                         srcDirs 'src/main/native/sim'
                         include '**/*.cpp'
-                        exclude '**/jni/*.cpp'
                     }
                     exportedHeaders {
                         srcDir 'src/main/native/include'
@@ -134,7 +105,6 @@
 }
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
-apply from: 'simjni.gradle'
 
 sourceSets.main.java.srcDir "${buildDir}/generated/java/"
 
@@ -150,12 +120,6 @@
     from('src/main/native/sim') {
         into '/sim'
     }
-
-    from("$buildDir/generated/cpp") {
-        into '/athena/jni'
-    }
-
-    dependsOn generateAthenaSimFiles
 }
 
 cppHeadersZip {

hal/simjni.gradle

diff --git a/hal/simjni.gradle b/hal/simjni.gradle
deleted file mode 100644
index c1aef73..0000000
--- a/hal/simjni.gradle
+++ /dev/null
@@ -1,111 +0,0 @@
-def athenaSimJniOutputDir = file("$buildDir/generated/cpp")
-def athenaSimJniOutputFile = file("$athenaSimJniOutputDir/simjni.cpp")
-
-task generateAthenaSimFiles() {
-    Set dirs = [];
-
-    def createdTask = it
-    outputs.file athenaSimJniOutputFile
-
-    model {
-        components {
-            it.all { component ->
-                if (component in getJniSpecClass()) {
-                    component.binaries.all { binary ->
-                        if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                            binary.tasks.withType(CppCompile) {
-                                it.dependsOn createdTask
-                            }
-                            component.jniHeaderLocations.each {
-                                dependsOn it.key
-                                createdTask.inputs.dir it.value
-                                dirs << it.value
-                            }
-                        }
-                    }
-                }
-            }
-        }
-    }
-
-    doLast {
-        def symbolList = []
-        dirs.each {
-            def tree = fileTree(dir: it)
-            tree.each { file ->
-                if (!file.name.contains('edu_wpi_first_hal_sim_mockdata_')) {
-                    return
-                }
-                boolean reading = false
-                String currentLine = ''
-                file.eachLine { line ->
-                    if (line.trim()) {
-                        if (line.contains(';') && reading) {
-                            currentLine += line.trim()
-                            reading = false
-                            symbolList << currentLine
-                            currentLine = ''
-                        }
-                        if (line.startsWith("JNIEXPORT ") && line.contains('JNICALL')) {
-                            if (line.contains(';')) {
-                                symbolList << line
-                                currentLine = ''
-                                reading = false
-                            } else {
-                                reading = true
-                                currentLine += line.trim()
-                            }
-
-                        }
-                    }
-                }
-            }
-        }
-        athenaSimJniOutputDir.mkdirs()
-        athenaSimJniOutputFile.withWriter { out ->
-            out.println '#include <jni.h>'
-            out.println '''
-static JavaVM* jvm = nullptr;
-
-namespace sim {
-jint SimOnLoad(JavaVM* vm, void* reserved) {
-  jvm = vm;
-
-  JNIEnv *env;
-  if (vm->GetEnv(reinterpret_cast<void **>(&env), JNI_VERSION_1_6) != JNI_OK)
-    return JNI_ERR;
-
-  return JNI_VERSION_1_6;
-}
-
-void SimOnUnload(JavaVM * vm, void* reserved) {
-  JNIEnv *env;
-  if (vm->GetEnv(reinterpret_cast<void **>(&env), JNI_VERSION_1_6) != JNI_OK)
-    return;
-  jvm = nullptr;
-}
-}
-
-static void ThrowSimException(JNIEnv* env) {
-
-}
-'''
-            out.println 'extern "C" {'
-            symbolList.each {
-                def symbol = it.replace('JNIEnv *', 'JNIEnv * env')
-                if (symbol.contains('JNIEXPORT void')) {
-                    symbol = symbol.replace(';', ''' {
-  ThrowSimException(env);
-}''')
-                } else {
-                    symbol = symbol.replace(';', ''' {
-  ThrowSimException(env);
-  return 0;
-}''')
-                }
-                out.println symbol
-            }
-            out.println '}'
-        }
-    }
-}

hal/src/dev/java/DevMain.java

diff --git a/hal/src/dev/java/DevMain.java b/hal/src/dev/java/DevMain.java
deleted file mode 100644
index 8cdb8ba..0000000
--- a/hal/src/dev/java/DevMain.java
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-public final class DevMain {
-  public static void main(String[] args) {
-
-  }
-
-  private DevMain() {
-  }
-}

hal/src/dev/java/edu/wpi/first/hal/DevMain.java

diff --git a/hal/src/dev/java/edu/wpi/first/hal/DevMain.java b/hal/src/dev/java/edu/wpi/first/hal/DevMain.java
new file mode 100644
index 0000000..25c6c90
--- /dev/null
+++ b/hal/src/dev/java/edu/wpi/first/hal/DevMain.java
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal;
+
+public final class DevMain {
+  public static void main(String[] args) {
+
+  }
+
+  private DevMain() {
+  }
+}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java b/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
index 029cc6d..3507569 100644
--- a/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
+++ b/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,6 +20,13 @@
 
   public static native void writeCANRTRFrame(int handle, int length, int apiId);
 
+  public static native int writeCANPacketNoThrow(int handle, byte[] data, int apiId);
+
+  public static native int writeCANPacketRepeatingNoThrow(int handle, byte[] data, int apiId,
+                                                         int repeatMs);
+
+  public static native int writeCANRTRFrameNoThrow(int handle, int length, int apiId);
+
   public static native void stopCANPacketRepeating(int handle, int apiId);
 
   public static native boolean readCANPacketNew(int handle, int apiId, CANData data);

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/HAL.java b/hal/src/main/java/edu/wpi/first/hal/HAL.java
index 8f09a0a..5e07488 100644
--- a/hal/src/main/java/edu/wpi/first/hal/HAL.java
+++ b/hal/src/main/java/edu/wpi/first/hal/HAL.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,12 +12,14 @@
 /**
  * JNI Wrapper for HAL<br>.
  */
-@SuppressWarnings({"AbbreviationAsWordInName", "MethodName", "PMD.TooManyMethods"})
+@SuppressWarnings({"AbbreviationAsWordInName", "MethodName"})
 public final class HAL extends JNIWrapper {
   public static native void waitForDSData();
 
   public static native boolean initialize(int timeout, int mode);
 
+  public static native void shutdown();
+
   public static native boolean hasMain();
 
   public static native void runMain();
@@ -130,6 +132,8 @@
                                      String details, String location, String callStack,
                                      boolean printMsg);
 
+  public static native int sendConsoleLine(String line);
+
   public static native int getPortWithModule(byte module, byte channel);
 
   public static native int getPort(byte channel);

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java b/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
index 8574a25..e370d5b 100644
--- a/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
+++ b/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -38,4 +38,6 @@
 
   public static native void setInterruptUpSourceEdge(int interruptHandle, boolean risingEdge,
                                                      boolean fallingEdge);
+
+  public static native void releaseWaitingInterrupt(int interruptHandle);
 }

hal/src/main/java/edu/wpi/first/hal/sim/AccelerometerSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/AccelerometerSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/AccelerometerSim.java
deleted file mode 100644
index d9c95af..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/AccelerometerSim.java
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.AccelerometerDataJNI;
-
-public class AccelerometerSim {
-  private final int m_index;
-
-  public AccelerometerSim() {
-    m_index = 0;
-  }
-
-  public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelActiveCallback);
-  }
-  public boolean getActive() {
-    return AccelerometerDataJNI.getActive(m_index);
-  }
-  public void setActive(boolean active) {
-    AccelerometerDataJNI.setActive(m_index, active);
-  }
-
-  public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelRangeCallback);
-  }
-  public int getRange() {
-    return AccelerometerDataJNI.getRange(m_index);
-  }
-  public void setRange(int range) {
-    AccelerometerDataJNI.setRange(m_index, range);
-  }
-
-  public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelXCallback);
-  }
-  public double getX() {
-    return AccelerometerDataJNI.getX(m_index);
-  }
-  public void setX(double x) {
-    AccelerometerDataJNI.setX(m_index, x);
-  }
-
-  public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelYCallback);
-  }
-  public double getY() {
-    return AccelerometerDataJNI.getY(m_index);
-  }
-  public void setY(double y) {
-    AccelerometerDataJNI.setY(m_index, y);
-  }
-
-  public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelZCallback);
-  }
-  public double getZ() {
-    return AccelerometerDataJNI.getZ(m_index);
-  }
-  public void setZ(double z) {
-    AccelerometerDataJNI.setZ(m_index, z);
-  }
-
-  public void resetData() {
-    AccelerometerDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/AddressableLEDSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/AddressableLEDSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/AddressableLEDSim.java
deleted file mode 100644
index cc6eefa..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/AddressableLEDSim.java
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.AddressableLEDDataJNI;
-
-public class AddressableLEDSim {
-  private final int m_index;
-
-  public AddressableLEDSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AddressableLEDDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return AddressableLEDDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    AddressableLEDDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerOutputPortCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AddressableLEDDataJNI.registerOutputPortCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelOutputPortCallback);
-  }
-  public int getOutputPort() {
-    return AddressableLEDDataJNI.getOutputPort(m_index);
-  }
-  public void setOutputPort(int outputPort) {
-    AddressableLEDDataJNI.setOutputPort(m_index, outputPort);
-  }
-
-  public CallbackStore registerLengthCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AddressableLEDDataJNI.registerLengthCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelLengthCallback);
-  }
-  public int getLength() {
-    return AddressableLEDDataJNI.getLength(m_index);
-  }
-  public void setLength(int length) {
-    AddressableLEDDataJNI.setLength(m_index, length);
-  }
-
-  public CallbackStore registerRunningCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AddressableLEDDataJNI.registerRunningCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelRunningCallback);
-  }
-  public boolean getRunning() {
-    return AddressableLEDDataJNI.getRunning(m_index);
-  }
-  public void setRunning(boolean running) {
-    AddressableLEDDataJNI.setRunning(m_index, running);
-  }
-
-  public CallbackStore registerDataCallback(ConstBufferCallback callback) {
-    int uid = AddressableLEDDataJNI.registerDataCallback(m_index, callback);
-    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelDataCallback);
-  }
-  public byte[] getData() {
-    return AddressableLEDDataJNI.getData(m_index);
-  }
-  public void setData(byte[] data) {
-    AddressableLEDDataJNI.setData(m_index, data);
-  }
-
-  public void resetData() {
-    AddressableLEDDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/AnalogGyroSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogGyroSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/AnalogGyroSim.java
deleted file mode 100644
index ddaa0ce..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogGyroSim.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.AnalogGyroDataJNI;
-
-public class AnalogGyroSim {
-  private final int m_index;
-
-  public AnalogGyroSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerAngleCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogGyroDataJNI.registerAngleCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelAngleCallback);
-  }
-  public double getAngle() {
-    return AnalogGyroDataJNI.getAngle(m_index);
-  }
-  public void setAngle(double angle) {
-    AnalogGyroDataJNI.setAngle(m_index, angle);
-  }
-
-  public CallbackStore registerRateCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogGyroDataJNI.registerRateCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelRateCallback);
-  }
-  public double getRate() {
-    return AnalogGyroDataJNI.getRate(m_index);
-  }
-  public void setRate(double rate) {
-    AnalogGyroDataJNI.setRate(m_index, rate);
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogGyroDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return AnalogGyroDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    AnalogGyroDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public void resetData() {
-    AnalogGyroDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/AnalogInSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogInSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/AnalogInSim.java
deleted file mode 100644
index f7f86bb..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogInSim.java
+++ /dev/null
@@ -1,121 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.AnalogInDataJNI;
-
-public class AnalogInSim {
-  private final int m_index;
-
-  public AnalogInSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return AnalogInDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    AnalogInDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerAverageBitsCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerAverageBitsCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAverageBitsCallback);
-  }
-  public int getAverageBits() {
-    return AnalogInDataJNI.getAverageBits(m_index);
-  }
-  public void setAverageBits(int averageBits) {
-    AnalogInDataJNI.setAverageBits(m_index, averageBits);
-  }
-
-  public CallbackStore registerOversampleBitsCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerOversampleBitsCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelOversampleBitsCallback);
-  }
-  public int getOversampleBits() {
-    return AnalogInDataJNI.getOversampleBits(m_index);
-  }
-  public void setOversampleBits(int oversampleBits) {
-    AnalogInDataJNI.setOversampleBits(m_index, oversampleBits);
-  }
-
-  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelVoltageCallback);
-  }
-  public double getVoltage() {
-    return AnalogInDataJNI.getVoltage(m_index);
-  }
-  public void setVoltage(double voltage) {
-    AnalogInDataJNI.setVoltage(m_index, voltage);
-  }
-
-  public CallbackStore registerAccumulatorInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerAccumulatorInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorInitializedCallback);
-  }
-  public boolean getAccumulatorInitialized() {
-    return AnalogInDataJNI.getAccumulatorInitialized(m_index);
-  }
-  public void setAccumulatorInitialized(boolean accumulatorInitialized) {
-    AnalogInDataJNI.setAccumulatorInitialized(m_index, accumulatorInitialized);
-  }
-
-  public CallbackStore registerAccumulatorValueCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerAccumulatorValueCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorValueCallback);
-  }
-  public long getAccumulatorValue() {
-    return AnalogInDataJNI.getAccumulatorValue(m_index);
-  }
-  public void setAccumulatorValue(long accumulatorValue) {
-    AnalogInDataJNI.setAccumulatorValue(m_index, accumulatorValue);
-  }
-
-  public CallbackStore registerAccumulatorCountCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerAccumulatorCountCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorCountCallback);
-  }
-  public long getAccumulatorCount() {
-    return AnalogInDataJNI.getAccumulatorCount(m_index);
-  }
-  public void setAccumulatorCount(long accumulatorCount) {
-    AnalogInDataJNI.setAccumulatorCount(m_index, accumulatorCount);
-  }
-
-  public CallbackStore registerAccumulatorCenterCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerAccumulatorCenterCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorCenterCallback);
-  }
-  public int getAccumulatorCenter() {
-    return AnalogInDataJNI.getAccumulatorCenter(m_index);
-  }
-  public void setAccumulatorCenter(int accumulatorCenter) {
-    AnalogInDataJNI.setAccumulatorCenter(m_index, accumulatorCenter);
-  }
-
-  public CallbackStore registerAccumulatorDeadbandCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerAccumulatorDeadbandCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorDeadbandCallback);
-  }
-  public int getAccumulatorDeadband() {
-    return AnalogInDataJNI.getAccumulatorDeadband(m_index);
-  }
-  public void setAccumulatorDeadband(int accumulatorDeadband) {
-    AnalogInDataJNI.setAccumulatorDeadband(m_index, accumulatorDeadband);
-  }
-
-  public void resetData() {
-    AnalogInDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/AnalogOutSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogOutSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/AnalogOutSim.java
deleted file mode 100644
index 4731b11..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogOutSim.java
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.AnalogOutDataJNI;
-
-public class AnalogOutSim {
-  private final int m_index;
-
-  public AnalogOutSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogOutDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogOutDataJNI::cancelVoltageCallback);
-  }
-  public double getVoltage() {
-    return AnalogOutDataJNI.getVoltage(m_index);
-  }
-  public void setVoltage(double voltage) {
-    AnalogOutDataJNI.setVoltage(m_index, voltage);
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogOutDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogOutDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return AnalogOutDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    AnalogOutDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public void resetData() {
-    AnalogOutDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/AnalogTriggerSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogTriggerSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/AnalogTriggerSim.java
deleted file mode 100644
index fd88d5f..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/AnalogTriggerSim.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.AnalogTriggerDataJNI;
-
-public class AnalogTriggerSim {
-  private final int m_index;
-
-  public AnalogTriggerSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogTriggerDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return AnalogTriggerDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    AnalogTriggerDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerTriggerLowerBoundCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogTriggerDataJNI.registerTriggerLowerBoundCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelTriggerLowerBoundCallback);
-  }
-  public double getTriggerLowerBound() {
-    return AnalogTriggerDataJNI.getTriggerLowerBound(m_index);
-  }
-  public void setTriggerLowerBound(double triggerLowerBound) {
-    AnalogTriggerDataJNI.setTriggerLowerBound(m_index, triggerLowerBound);
-  }
-
-  public CallbackStore registerTriggerUpperBoundCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogTriggerDataJNI.registerTriggerUpperBoundCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelTriggerUpperBoundCallback);
-  }
-  public double getTriggerUpperBound() {
-    return AnalogTriggerDataJNI.getTriggerUpperBound(m_index);
-  }
-  public void setTriggerUpperBound(double triggerUpperBound) {
-    AnalogTriggerDataJNI.setTriggerUpperBound(m_index, triggerUpperBound);
-  }
-
-  public void resetData() {
-    AnalogTriggerDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/BufferCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/BufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/sim/BufferCallback.java
deleted file mode 100644
index 0f7b2d9..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/BufferCallback.java
+++ /dev/null
@@ -1,12 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-public interface BufferCallback {
-  void callback(String name, byte[] buffer, int count);
-}

hal/src/main/java/edu/wpi/first/hal/sim/CallbackStore.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/CallbackStore.java b/hal/src/main/java/edu/wpi/first/hal/sim/CallbackStore.java
deleted file mode 100644
index 7564104..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/CallbackStore.java
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-public class CallbackStore implements AutoCloseable {
-  interface CancelCallbackFunc {
-    void cancel(int index, int uid);
-  }
-
-  interface CancelCallbackChannelFunc {
-    void cancel(int index, int channel, int uid);
-  }
-
-  interface CancelCallbackNoIndexFunc {
-    void cancel(int uid);
-  }
-
-  public CallbackStore(int index, int uid, CancelCallbackFunc ccf) {
-    this.m_cancelType = kNormalCancel;
-    this.m_index = index;
-    this.m_uid = uid;
-    this.m_cancelCallback = ccf;
-  }
-
-  public CallbackStore(int index, int channel, int uid, CancelCallbackChannelFunc ccf) {
-    this.m_cancelType = kChannelCancel;
-    this.m_index = index;
-    this.m_uid = uid;
-    this.m_channel = channel;
-    this.m_cancelCallbackChannel = ccf;
-  }
-
-  public CallbackStore(int uid, CancelCallbackNoIndexFunc ccf) {
-    this.m_cancelType = kNoIndexCancel;
-    this.m_uid = uid;
-    this.m_cancelCallbackNoIndex = ccf;
-  }
-
-  private int m_index;
-  private int m_channel;
-  private final int m_uid;
-  private CancelCallbackFunc m_cancelCallback;
-  private CancelCallbackChannelFunc m_cancelCallbackChannel;
-  private CancelCallbackNoIndexFunc m_cancelCallbackNoIndex;
-  private static final int kNormalCancel = 0;
-  private static final int kChannelCancel = 1;
-  private static final int kNoIndexCancel = 2;
-  private int m_cancelType;
-
-  @Override
-  public void close() {
-    switch (m_cancelType) {
-      case kNormalCancel:
-        m_cancelCallback.cancel(m_index, m_uid);
-        break;
-      case kChannelCancel:
-        m_cancelCallbackChannel.cancel(m_index, m_channel, m_uid);
-        break;
-      case kNoIndexCancel:
-        m_cancelCallbackNoIndex.cancel(m_uid);
-        break;
-      default:
-        assert false;
-        break;
-    }
-    m_cancelType = -1;
-  }
-
-  @Override
-  protected void finalize() throws Throwable {
-    try {
-      if (m_cancelType >= 0) {
-        close();        // close open files
-      }
-    } finally {
-      super.finalize();
-    }
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/ConstBufferCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/ConstBufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/sim/ConstBufferCallback.java
deleted file mode 100644
index a4251c2..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/ConstBufferCallback.java
+++ /dev/null
@@ -1,12 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-public interface ConstBufferCallback {
-  void callback(String name, byte[] buffer, int count);
-}

hal/src/main/java/edu/wpi/first/hal/sim/DIOSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/DIOSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/DIOSim.java
deleted file mode 100644
index cd75822..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/DIOSim.java
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.DIODataJNI;
-
-public class DIOSim {
-  private final int m_index;
-
-  public DIOSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DIODataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DIODataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return DIODataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    DIODataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerValueCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DIODataJNI.registerValueCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DIODataJNI::cancelValueCallback);
-  }
-  public boolean getValue() {
-    return DIODataJNI.getValue(m_index);
-  }
-  public void setValue(boolean value) {
-    DIODataJNI.setValue(m_index, value);
-  }
-
-  public CallbackStore registerPulseLengthCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DIODataJNI.registerPulseLengthCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DIODataJNI::cancelPulseLengthCallback);
-  }
-  public double getPulseLength() {
-    return DIODataJNI.getPulseLength(m_index);
-  }
-  public void setPulseLength(double pulseLength) {
-    DIODataJNI.setPulseLength(m_index, pulseLength);
-  }
-
-  public CallbackStore registerIsInputCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DIODataJNI.registerIsInputCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DIODataJNI::cancelIsInputCallback);
-  }
-  public boolean getIsInput() {
-    return DIODataJNI.getIsInput(m_index);
-  }
-  public void setIsInput(boolean isInput) {
-    DIODataJNI.setIsInput(m_index, isInput);
-  }
-
-  public CallbackStore registerFilterIndexCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DIODataJNI.registerFilterIndexCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DIODataJNI::cancelFilterIndexCallback);
-  }
-  public int getFilterIndex() {
-    return DIODataJNI.getFilterIndex(m_index);
-  }
-  public void setFilterIndex(int filterIndex) {
-    DIODataJNI.setFilterIndex(m_index, filterIndex);
-  }
-
-  public void resetData() {
-    DIODataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/DigitalPWMSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/DigitalPWMSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/DigitalPWMSim.java
deleted file mode 100644
index 314d994..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/DigitalPWMSim.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.DigitalPWMDataJNI;
-
-public class DigitalPWMSim {
-  private final int m_index;
-
-  public DigitalPWMSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DigitalPWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return DigitalPWMDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    DigitalPWMDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerDutyCycleCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DigitalPWMDataJNI.registerDutyCycleCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelDutyCycleCallback);
-  }
-  public double getDutyCycle() {
-    return DigitalPWMDataJNI.getDutyCycle(m_index);
-  }
-  public void setDutyCycle(double dutyCycle) {
-    DigitalPWMDataJNI.setDutyCycle(m_index, dutyCycle);
-  }
-
-  public CallbackStore registerPinCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DigitalPWMDataJNI.registerPinCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelPinCallback);
-  }
-  public int getPin() {
-    return DigitalPWMDataJNI.getPin(m_index);
-  }
-  public void setPin(int pin) {
-    DigitalPWMDataJNI.setPin(m_index, pin);
-  }
-
-  public void resetData() {
-    DigitalPWMDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java
deleted file mode 100644
index d14af9f..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/DriverStationSim.java
+++ /dev/null
@@ -1,94 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.DriverStationDataJNI;
-
-public class DriverStationSim {
-  public CallbackStore registerEnabledCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DriverStationDataJNI.registerEnabledCallback(callback, initialNotify);
-    return new CallbackStore(uid, DriverStationDataJNI::cancelEnabledCallback);
-  }
-  public boolean getEnabled() {
-    return DriverStationDataJNI.getEnabled();
-  }
-  public void setEnabled(boolean enabled) {
-    DriverStationDataJNI.setEnabled(enabled);
-  }
-
-  public CallbackStore registerAutonomousCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DriverStationDataJNI.registerAutonomousCallback(callback, initialNotify);
-    return new CallbackStore(uid, DriverStationDataJNI::cancelAutonomousCallback);
-  }
-  public boolean getAutonomous() {
-    return DriverStationDataJNI.getAutonomous();
-  }
-  public void setAutonomous(boolean autonomous) {
-    DriverStationDataJNI.setAutonomous(autonomous);
-  }
-
-  public CallbackStore registerTestCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DriverStationDataJNI.registerTestCallback(callback, initialNotify);
-    return new CallbackStore(uid, DriverStationDataJNI::cancelTestCallback);
-  }
-  public boolean getTest() {
-    return DriverStationDataJNI.getTest();
-  }
-  public void setTest(boolean test) {
-    DriverStationDataJNI.setTest(test);
-  }
-
-  public CallbackStore registerEStopCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DriverStationDataJNI.registerEStopCallback(callback, initialNotify);
-    return new CallbackStore(uid, DriverStationDataJNI::cancelEStopCallback);
-  }
-  public boolean getEStop() {
-    return DriverStationDataJNI.getEStop();
-  }
-  public void setEStop(boolean eStop) {
-    DriverStationDataJNI.setEStop(eStop);
-  }
-
-  public CallbackStore registerFmsAttachedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DriverStationDataJNI.registerFmsAttachedCallback(callback, initialNotify);
-    return new CallbackStore(uid, DriverStationDataJNI::cancelFmsAttachedCallback);
-  }
-  public boolean getFmsAttached() {
-    return DriverStationDataJNI.getFmsAttached();
-  }
-  public void setFmsAttached(boolean fmsAttached) {
-    DriverStationDataJNI.setFmsAttached(fmsAttached);
-  }
-
-  public CallbackStore registerDsAttachedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DriverStationDataJNI.registerDsAttachedCallback(callback, initialNotify);
-    return new CallbackStore(uid, DriverStationDataJNI::cancelDsAttachedCallback);
-  }
-  public boolean getDsAttached() {
-    return DriverStationDataJNI.getDsAttached();
-  }
-  public void setDsAttached(boolean dsAttached) {
-    DriverStationDataJNI.setDsAttached(dsAttached);
-  }
-  public void notifyNewData() {
-    DriverStationDataJNI.notifyNewData();
-  }
-
-  /**
-   * Toggles suppression of DriverStation.reportError and reportWarning messages.
-   *
-   * @param shouldSend If false then messages will will be suppressed.
-   */
-  public void setSendError(boolean shouldSend) {
-    DriverStationDataJNI.setSendError(shouldSend);
-  }
-
-  public void resetData() {
-    DriverStationDataJNI.resetData();
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/DutyCycleSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/DutyCycleSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/DutyCycleSim.java
deleted file mode 100644
index 9b2806b..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/DutyCycleSim.java
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.DutyCycleDataJNI;
-
-public class DutyCycleSim {
-  private final int m_index;
-
-  public DutyCycleSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DutyCycleDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return DutyCycleDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    DutyCycleDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerFrequencyCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DutyCycleDataJNI.registerFrequencyCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelFrequencyCallback);
-  }
-  public int getFrequency() {
-    return DutyCycleDataJNI.getFrequency(m_index);
-  }
-  public void setFrequency(int frequency) {
-    DutyCycleDataJNI.setFrequency(m_index, frequency);
-  }
-
-  public CallbackStore registerOutputCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = DutyCycleDataJNI.registerOutputCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelOutputCallback);
-  }
-  public double getOutput() {
-    return DutyCycleDataJNI.getOutput(m_index);
-  }
-  public void setOutput(double output) {
-    DutyCycleDataJNI.setOutput(m_index, output);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/EncoderSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/EncoderSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/EncoderSim.java
deleted file mode 100644
index 408ca84..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/EncoderSim.java
+++ /dev/null
@@ -1,110 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.EncoderDataJNI;
-
-public class EncoderSim {
-  private final int m_index;
-
-  public EncoderSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return EncoderDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    EncoderDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerCountCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerCountCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelCountCallback);
-  }
-  public int getCount() {
-    return EncoderDataJNI.getCount(m_index);
-  }
-  public void setCount(int count) {
-    EncoderDataJNI.setCount(m_index, count);
-  }
-
-  public CallbackStore registerPeriodCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerPeriodCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelPeriodCallback);
-  }
-  public double getPeriod() {
-    return EncoderDataJNI.getPeriod(m_index);
-  }
-  public void setPeriod(double period) {
-    EncoderDataJNI.setPeriod(m_index, period);
-  }
-
-  public CallbackStore registerResetCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerResetCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelResetCallback);
-  }
-  public boolean getReset() {
-    return EncoderDataJNI.getReset(m_index);
-  }
-  public void setReset(boolean reset) {
-    EncoderDataJNI.setReset(m_index, reset);
-  }
-
-  public CallbackStore registerMaxPeriodCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerMaxPeriodCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelMaxPeriodCallback);
-  }
-  public double getMaxPeriod() {
-    return EncoderDataJNI.getMaxPeriod(m_index);
-  }
-  public void setMaxPeriod(double maxPeriod) {
-    EncoderDataJNI.setMaxPeriod(m_index, maxPeriod);
-  }
-
-  public CallbackStore registerDirectionCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerDirectionCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelDirectionCallback);
-  }
-  public boolean getDirection() {
-    return EncoderDataJNI.getDirection(m_index);
-  }
-  public void setDirection(boolean direction) {
-    EncoderDataJNI.setDirection(m_index, direction);
-  }
-
-  public CallbackStore registerReverseDirectionCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerReverseDirectionCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelReverseDirectionCallback);
-  }
-  public boolean getReverseDirection() {
-    return EncoderDataJNI.getReverseDirection(m_index);
-  }
-  public void setReverseDirection(boolean reverseDirection) {
-    EncoderDataJNI.setReverseDirection(m_index, reverseDirection);
-  }
-
-  public CallbackStore registerSamplesToAverageCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = EncoderDataJNI.registerSamplesToAverageCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelSamplesToAverageCallback);
-  }
-  public int getSamplesToAverage() {
-    return EncoderDataJNI.getSamplesToAverage(m_index);
-  }
-  public void setSamplesToAverage(int samplesToAverage) {
-    EncoderDataJNI.setSamplesToAverage(m_index, samplesToAverage);
-  }
-
-  public void resetData() {
-    EncoderDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/I2CSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/I2CSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/I2CSim.java
deleted file mode 100644
index 3a9aa02..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/I2CSim.java
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.I2CDataJNI;
-
-public class I2CSim {
-  private final int m_index;
-
-  public I2CSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = I2CDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, I2CDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return I2CDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    I2CDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerReadCallback(BufferCallback callback) {
-    int uid = I2CDataJNI.registerReadCallback(m_index, callback);
-    return new CallbackStore(m_index, uid, I2CDataJNI::cancelReadCallback);
-  }
-
-  public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
-    int uid = I2CDataJNI.registerWriteCallback(m_index, callback);
-    return new CallbackStore(m_index, uid, I2CDataJNI::cancelWriteCallback);
-  }
-
-  public void resetData() {
-    I2CDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/NotifierSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/NotifierSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/NotifierSim.java
deleted file mode 100644
index f19a674..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/NotifierSim.java
+++ /dev/null
@@ -1,23 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.NotifierDataJNI;
-
-public final class NotifierSim {
-  private NotifierSim() {
-  }
-
-  public static long getNextTimeout() {
-    return NotifierDataJNI.getNextTimeout();
-  }
-
-  public static int getNumNotifiers() {
-    return NotifierDataJNI.getNumNotifiers();
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/NotifyCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/NotifyCallback.java b/hal/src/main/java/edu/wpi/first/hal/sim/NotifyCallback.java
deleted file mode 100644
index 9c655e4..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/NotifyCallback.java
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.HALValue;
-
-public interface NotifyCallback {
-  void callback(String name, HALValue value);
-
-  default void callbackNative(String name, int type, long value1, double value2) {
-    callback(name, HALValue.fromNative(type, value1, value2));
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/PCMSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/PCMSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/PCMSim.java
deleted file mode 100644
index f8cc327..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/PCMSim.java
+++ /dev/null
@@ -1,99 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.PCMDataJNI;
-
-public class PCMSim {
-  private final int m_index;
-
-  public PCMSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerSolenoidInitializedCallback(int channel, NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerSolenoidInitializedCallback(m_index, channel, callback, initialNotify);
-    return new CallbackStore(m_index, channel, uid, PCMDataJNI::cancelSolenoidInitializedCallback);
-  }
-  public boolean getSolenoidInitialized(int channel) {
-    return PCMDataJNI.getSolenoidInitialized(m_index, channel);
-  }
-  public void setSolenoidInitialized(int channel, boolean solenoidInitialized) {
-    PCMDataJNI.setSolenoidInitialized(m_index, channel, solenoidInitialized);
-  }
-
-  public CallbackStore registerSolenoidOutputCallback(int channel, NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify);
-    return new CallbackStore(m_index, channel, uid, PCMDataJNI::cancelSolenoidOutputCallback);
-  }
-  public boolean getSolenoidOutput(int channel) {
-    return PCMDataJNI.getSolenoidOutput(m_index, channel);
-  }
-  public void setSolenoidOutput(int channel, boolean solenoidOutput) {
-    PCMDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput);
-  }
-
-  public CallbackStore registerCompressorInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerCompressorInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorInitializedCallback);
-  }
-  public boolean getCompressorInitialized() {
-    return PCMDataJNI.getCompressorInitialized(m_index);
-  }
-  public void setCompressorInitialized(boolean compressorInitialized) {
-    PCMDataJNI.setCompressorInitialized(m_index, compressorInitialized);
-  }
-
-  public CallbackStore registerCompressorOnCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorOnCallback);
-  }
-  public boolean getCompressorOn() {
-    return PCMDataJNI.getCompressorOn(m_index);
-  }
-  public void setCompressorOn(boolean compressorOn) {
-    PCMDataJNI.setCompressorOn(m_index, compressorOn);
-  }
-
-  public CallbackStore registerClosedLoopEnabledCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerClosedLoopEnabledCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelClosedLoopEnabledCallback);
-  }
-  public boolean getClosedLoopEnabled() {
-    return PCMDataJNI.getClosedLoopEnabled(m_index);
-  }
-  public void setClosedLoopEnabled(boolean closedLoopEnabled) {
-    PCMDataJNI.setClosedLoopEnabled(m_index, closedLoopEnabled);
-  }
-
-  public CallbackStore registerPressureSwitchCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelPressureSwitchCallback);
-  }
-  public boolean getPressureSwitch() {
-    return PCMDataJNI.getPressureSwitch(m_index);
-  }
-  public void setPressureSwitch(boolean pressureSwitch) {
-    PCMDataJNI.setPressureSwitch(m_index, pressureSwitch);
-  }
-
-  public CallbackStore registerCompressorCurrentCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorCurrentCallback);
-  }
-  public double getCompressorCurrent() {
-    return PCMDataJNI.getCompressorCurrent(m_index);
-  }
-  public void setCompressorCurrent(double compressorCurrent) {
-    PCMDataJNI.setCompressorCurrent(m_index, compressorCurrent);
-  }
-
-  public void resetData() {
-    PCMDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/PDPSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/PDPSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/PDPSim.java
deleted file mode 100644
index d44cf3a..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/PDPSim.java
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.PDPDataJNI;
-
-public class PDPSim {
-  private final int m_index;
-
-  public PDPSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PDPDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return PDPDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    PDPDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerTemperatureCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerTemperatureCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PDPDataJNI::cancelTemperatureCallback);
-  }
-  public double getTemperature() {
-    return PDPDataJNI.getTemperature(m_index);
-  }
-  public void setTemperature(double temperature) {
-    PDPDataJNI.setTemperature(m_index, temperature);
-  }
-
-  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PDPDataJNI::cancelVoltageCallback);
-  }
-  public double getVoltage() {
-    return PDPDataJNI.getVoltage(m_index);
-  }
-  public void setVoltage(double voltage) {
-    PDPDataJNI.setVoltage(m_index, voltage);
-  }
-
-  public CallbackStore registerCurrentCallback(int channel, NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerCurrentCallback(m_index, channel, callback, initialNotify);
-    return new CallbackStore(m_index, channel, uid, PDPDataJNI::cancelCurrentCallback);
-  }
-  public double getCurrent(int channel) {
-    return PDPDataJNI.getCurrent(m_index, channel);
-  }
-  public void setCurrent(int channel, double current) {
-    PDPDataJNI.setCurrent(m_index, channel, current);
-  }
-
-  public void resetData() {
-    PDPDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/PWMSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/PWMSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/PWMSim.java
deleted file mode 100644
index dde38a4..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/PWMSim.java
+++ /dev/null
@@ -1,88 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.PWMDataJNI;
-
-public class PWMSim {
-  private final int m_index;
-
-  public PWMSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PWMDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return PWMDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    PWMDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerRawValueCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PWMDataJNI.registerRawValueCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PWMDataJNI::cancelRawValueCallback);
-  }
-  public int getRawValue() {
-    return PWMDataJNI.getRawValue(m_index);
-  }
-  public void setRawValue(int rawValue) {
-    PWMDataJNI.setRawValue(m_index, rawValue);
-  }
-
-  public CallbackStore registerSpeedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PWMDataJNI.registerSpeedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PWMDataJNI::cancelSpeedCallback);
-  }
-  public double getSpeed() {
-    return PWMDataJNI.getSpeed(m_index);
-  }
-  public void setSpeed(double speed) {
-    PWMDataJNI.setSpeed(m_index, speed);
-  }
-
-  public CallbackStore registerPositionCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PWMDataJNI.registerPositionCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PWMDataJNI::cancelPositionCallback);
-  }
-  public double getPosition() {
-    return PWMDataJNI.getPosition(m_index);
-  }
-  public void setPosition(double position) {
-    PWMDataJNI.setPosition(m_index, position);
-  }
-
-  public CallbackStore registerPeriodScaleCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PWMDataJNI.registerPeriodScaleCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PWMDataJNI::cancelPeriodScaleCallback);
-  }
-  public int getPeriodScale() {
-    return PWMDataJNI.getPeriodScale(m_index);
-  }
-  public void setPeriodScale(int periodScale) {
-    PWMDataJNI.setPeriodScale(m_index, periodScale);
-  }
-
-  public CallbackStore registerZeroLatchCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PWMDataJNI.registerZeroLatchCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PWMDataJNI::cancelZeroLatchCallback);
-  }
-  public boolean getZeroLatch() {
-    return PWMDataJNI.getZeroLatch(m_index);
-  }
-  public void setZeroLatch(boolean zeroLatch) {
-    PWMDataJNI.setZeroLatch(m_index, zeroLatch);
-  }
-
-  public void resetData() {
-    PWMDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/RelaySim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/RelaySim.java b/hal/src/main/java/edu/wpi/first/hal/sim/RelaySim.java
deleted file mode 100644
index 84059e8..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/RelaySim.java
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.RelayDataJNI;
-
-public class RelaySim {
-  private final int m_index;
-
-  public RelaySim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedForwardCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RelayDataJNI.registerInitializedForwardCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RelayDataJNI::cancelInitializedForwardCallback);
-  }
-  public boolean getInitializedForward() {
-    return RelayDataJNI.getInitializedForward(m_index);
-  }
-  public void setInitializedForward(boolean initializedForward) {
-    RelayDataJNI.setInitializedForward(m_index, initializedForward);
-  }
-
-  public CallbackStore registerInitializedReverseCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RelayDataJNI.registerInitializedReverseCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RelayDataJNI::cancelInitializedReverseCallback);
-  }
-  public boolean getInitializedReverse() {
-    return RelayDataJNI.getInitializedReverse(m_index);
-  }
-  public void setInitializedReverse(boolean initializedReverse) {
-    RelayDataJNI.setInitializedReverse(m_index, initializedReverse);
-  }
-
-  public CallbackStore registerForwardCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RelayDataJNI.registerForwardCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RelayDataJNI::cancelForwardCallback);
-  }
-  public boolean getForward() {
-    return RelayDataJNI.getForward(m_index);
-  }
-  public void setForward(boolean forward) {
-    RelayDataJNI.setForward(m_index, forward);
-  }
-
-  public CallbackStore registerReverseCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RelayDataJNI.registerReverseCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RelayDataJNI::cancelReverseCallback);
-  }
-  public boolean getReverse() {
-    return RelayDataJNI.getReverse(m_index);
-  }
-  public void setReverse(boolean reverse) {
-    RelayDataJNI.setReverse(m_index, reverse);
-  }
-
-  public void resetData() {
-    RelayDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/RoboRioSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/RoboRioSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/RoboRioSim.java
deleted file mode 100644
index 082f712..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/RoboRioSim.java
+++ /dev/null
@@ -1,188 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.RoboRioDataJNI;
-
-@SuppressWarnings({"PMD.ExcessivePublicCount", "PMD.TooManyMethods"})
-public class RoboRioSim {
-  private final int m_index;
-
-  public RoboRioSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerFPGAButtonCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerFPGAButtonCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelFPGAButtonCallback);
-  }
-  public boolean getFPGAButton() {
-    return RoboRioDataJNI.getFPGAButton(m_index);
-  }
-  public void setFPGAButton(boolean fPGAButton) {
-    RoboRioDataJNI.setFPGAButton(m_index, fPGAButton);
-  }
-
-  public CallbackStore registerVInVoltageCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerVInVoltageCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelVInVoltageCallback);
-  }
-  public double getVInVoltage() {
-    return RoboRioDataJNI.getVInVoltage(m_index);
-  }
-  public void setVInVoltage(double vInVoltage) {
-    RoboRioDataJNI.setVInVoltage(m_index, vInVoltage);
-  }
-
-  public CallbackStore registerVInCurrentCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerVInCurrentCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelVInCurrentCallback);
-  }
-  public double getVInCurrent() {
-    return RoboRioDataJNI.getVInCurrent(m_index);
-  }
-  public void setVInCurrent(double vInCurrent) {
-    RoboRioDataJNI.setVInCurrent(m_index, vInCurrent);
-  }
-
-  public CallbackStore registerUserVoltage6VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserVoltage6VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserVoltage6VCallback);
-  }
-  public double getUserVoltage6V() {
-    return RoboRioDataJNI.getUserVoltage6V(m_index);
-  }
-  public void setUserVoltage6V(double userVoltage6V) {
-    RoboRioDataJNI.setUserVoltage6V(m_index, userVoltage6V);
-  }
-
-  public CallbackStore registerUserCurrent6VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserCurrent6VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserCurrent6VCallback);
-  }
-  public double getUserCurrent6V() {
-    return RoboRioDataJNI.getUserCurrent6V(m_index);
-  }
-  public void setUserCurrent6V(double userCurrent6V) {
-    RoboRioDataJNI.setUserCurrent6V(m_index, userCurrent6V);
-  }
-
-  public CallbackStore registerUserActive6VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserActive6VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserActive6VCallback);
-  }
-  public boolean getUserActive6V() {
-    return RoboRioDataJNI.getUserActive6V(m_index);
-  }
-  public void setUserActive6V(boolean userActive6V) {
-    RoboRioDataJNI.setUserActive6V(m_index, userActive6V);
-  }
-
-  public CallbackStore registerUserVoltage5VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserVoltage5VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserVoltage5VCallback);
-  }
-  public double getUserVoltage5V() {
-    return RoboRioDataJNI.getUserVoltage5V(m_index);
-  }
-  public void setUserVoltage5V(double userVoltage5V) {
-    RoboRioDataJNI.setUserVoltage5V(m_index, userVoltage5V);
-  }
-
-  public CallbackStore registerUserCurrent5VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserCurrent5VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserCurrent5VCallback);
-  }
-  public double getUserCurrent5V() {
-    return RoboRioDataJNI.getUserCurrent5V(m_index);
-  }
-  public void setUserCurrent5V(double userCurrent5V) {
-    RoboRioDataJNI.setUserCurrent5V(m_index, userCurrent5V);
-  }
-
-  public CallbackStore registerUserActive5VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserActive5VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserActive5VCallback);
-  }
-  public boolean getUserActive5V() {
-    return RoboRioDataJNI.getUserActive5V(m_index);
-  }
-  public void setUserActive5V(boolean userActive5V) {
-    RoboRioDataJNI.setUserActive5V(m_index, userActive5V);
-  }
-
-  public CallbackStore registerUserVoltage3V3Callback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserVoltage3V3Callback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserVoltage3V3Callback);
-  }
-  public double getUserVoltage3V3() {
-    return RoboRioDataJNI.getUserVoltage3V3(m_index);
-  }
-  public void setUserVoltage3V3(double userVoltage3V3) {
-    RoboRioDataJNI.setUserVoltage3V3(m_index, userVoltage3V3);
-  }
-
-  public CallbackStore registerUserCurrent3V3Callback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserCurrent3V3Callback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserCurrent3V3Callback);
-  }
-  public double getUserCurrent3V3() {
-    return RoboRioDataJNI.getUserCurrent3V3(m_index);
-  }
-  public void setUserCurrent3V3(double userCurrent3V3) {
-    RoboRioDataJNI.setUserCurrent3V3(m_index, userCurrent3V3);
-  }
-
-  public CallbackStore registerUserActive3V3Callback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserActive3V3Callback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserActive3V3Callback);
-  }
-  public boolean getUserActive3V3() {
-    return RoboRioDataJNI.getUserActive3V3(m_index);
-  }
-  public void setUserActive3V3(boolean userActive3V3) {
-    RoboRioDataJNI.setUserActive3V3(m_index, userActive3V3);
-  }
-
-  public CallbackStore registerUserFaults6VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserFaults6VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserFaults6VCallback);
-  }
-  public int getUserFaults6V() {
-    return RoboRioDataJNI.getUserFaults6V(m_index);
-  }
-  public void setUserFaults6V(int userFaults6V) {
-    RoboRioDataJNI.setUserFaults6V(m_index, userFaults6V);
-  }
-
-  public CallbackStore registerUserFaults5VCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserFaults5VCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserFaults5VCallback);
-  }
-  public int getUserFaults5V() {
-    return RoboRioDataJNI.getUserFaults5V(m_index);
-  }
-  public void setUserFaults5V(int userFaults5V) {
-    RoboRioDataJNI.setUserFaults5V(m_index, userFaults5V);
-  }
-
-  public CallbackStore registerUserFaults3V3Callback(NotifyCallback callback, boolean initialNotify) {
-    int uid = RoboRioDataJNI.registerUserFaults3V3Callback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, RoboRioDataJNI::cancelUserFaults3V3Callback);
-  }
-  public int getUserFaults3V3() {
-    return RoboRioDataJNI.getUserFaults3V3(m_index);
-  }
-  public void setUserFaults3V3(int userFaults3V3) {
-    RoboRioDataJNI.setUserFaults3V3(m_index, userFaults3V3);
-  }
-
-  public void resetData() {
-    RoboRioDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/SPIAccelerometerSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SPIAccelerometerSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/SPIAccelerometerSim.java
deleted file mode 100644
index 94577bd..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SPIAccelerometerSim.java
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.SPIAccelerometerDataJNI;
-
-public class SPIAccelerometerSim {
-  private final int m_index;
-
-  public SPIAccelerometerSim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = SPIAccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelActiveCallback);
-  }
-  public boolean getActive() {
-    return SPIAccelerometerDataJNI.getActive(m_index);
-  }
-  public void setActive(boolean active) {
-    SPIAccelerometerDataJNI.setActive(m_index, active);
-  }
-
-  public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = SPIAccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelRangeCallback);
-  }
-  public int getRange() {
-    return SPIAccelerometerDataJNI.getRange(m_index);
-  }
-  public void setRange(int range) {
-    SPIAccelerometerDataJNI.setRange(m_index, range);
-  }
-
-  public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = SPIAccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelXCallback);
-  }
-  public double getX() {
-    return SPIAccelerometerDataJNI.getX(m_index);
-  }
-  public void setX(double x) {
-    SPIAccelerometerDataJNI.setX(m_index, x);
-  }
-
-  public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = SPIAccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelYCallback);
-  }
-  public double getY() {
-    return SPIAccelerometerDataJNI.getY(m_index);
-  }
-  public void setY(double y) {
-    SPIAccelerometerDataJNI.setY(m_index, y);
-  }
-
-  public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = SPIAccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelZCallback);
-  }
-  public double getZ() {
-    return SPIAccelerometerDataJNI.getZ(m_index);
-  }
-  public void setZ(double z) {
-    SPIAccelerometerDataJNI.setZ(m_index, z);
-  }
-
-  public void resetData() {
-    SPIAccelerometerDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/SPISim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SPISim.java b/hal/src/main/java/edu/wpi/first/hal/sim/SPISim.java
deleted file mode 100644
index 5f43bca..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SPISim.java
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.SPIDataJNI;
-
-public class SPISim {
-  private final int m_index;
-
-  public SPISim(int index) {
-    m_index = index;
-  }
-
-  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = SPIDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, SPIDataJNI::cancelInitializedCallback);
-  }
-  public boolean getInitialized() {
-    return SPIDataJNI.getInitialized(m_index);
-  }
-  public void setInitialized(boolean initialized) {
-    SPIDataJNI.setInitialized(m_index, initialized);
-  }
-
-  public CallbackStore registerReadCallback(BufferCallback callback) {
-    int uid = SPIDataJNI.registerReadCallback(m_index, callback);
-    return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadCallback);
-  }
-
-  public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
-    int uid = SPIDataJNI.registerWriteCallback(m_index, callback);
-    return new CallbackStore(m_index, uid, SPIDataJNI::cancelWriteCallback);
-  }
-
-  public CallbackStore registerReadAutoReceiveBufferCallback(SpiReadAutoReceiveBufferCallback callback) {
-    int uid = SPIDataJNI.registerReadAutoReceiveBufferCallback(m_index, callback);
-    return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadAutoReceiveBufferCallback);
-  }
-
-  public void resetData() {
-    SPIDataJNI.resetData(m_index);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceCallback.java b/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceCallback.java
deleted file mode 100644
index ff9e45b..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceCallback.java
+++ /dev/null
@@ -1,13 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-@FunctionalInterface
-public interface SimDeviceCallback {
-  void callback(String name, int handle);
-}

hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceSim.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceSim.java b/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceSim.java
deleted file mode 100644
index e46c811..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SimDeviceSim.java
+++ /dev/null
@@ -1,94 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.SimBoolean;
-import edu.wpi.first.hal.SimDouble;
-import edu.wpi.first.hal.SimEnum;
-import edu.wpi.first.hal.SimValue;
-import edu.wpi.first.hal.sim.mockdata.SimDeviceDataJNI;
-
-public class SimDeviceSim {
-  private final int m_handle;
-
-  public SimDeviceSim(String name) {
-    m_handle = SimDeviceDataJNI.getSimDeviceHandle(name);
-  }
-
-  public SimValue getValue(String name) {
-    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
-    if (handle <= 0) {
-      return null;
-    }
-    return new SimValue(handle);
-  }
-
-  public SimDouble getDouble(String name) {
-    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
-    if (handle <= 0) {
-      return null;
-    }
-    return new SimDouble(handle);
-  }
-
-  public SimEnum getEnum(String name) {
-    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
-    if (handle <= 0) {
-      return null;
-    }
-    return new SimEnum(handle);
-  }
-
-  public SimBoolean getBoolean(String name) {
-    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
-    if (handle <= 0) {
-      return null;
-    }
-    return new SimBoolean(handle);
-  }
-
-  public static String[] getEnumOptions(SimEnum val) {
-    return SimDeviceDataJNI.getSimValueEnumOptions(val.getNativeHandle());
-  }
-
-  public SimDeviceDataJNI.SimValueInfo[] enumerateValues() {
-    return SimDeviceDataJNI.enumerateSimValues(m_handle);
-  }
-
-  public int getNativeHandle() {
-    return m_handle;
-  }
-
-  public CallbackStore registerValueCreatedCallback(SimValueCallback callback, boolean initialNotify) {
-    int uid = SimDeviceDataJNI.registerSimValueCreatedCallback(m_handle, callback, initialNotify);
-    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueCreatedCallback);
-  }
-
-  public CallbackStore registerValueChangedCallback(SimValueCallback callback, boolean initialNotify) {
-    int uid = SimDeviceDataJNI.registerSimValueChangedCallback(m_handle, callback, initialNotify);
-    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
-  }
-
-  public static SimDeviceDataJNI.SimDeviceInfo[] enumerateDevices(String prefix) {
-    return SimDeviceDataJNI.enumerateSimDevices(prefix);
-  }
-
-  public CallbackStore registerDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify) {
-    int uid = SimDeviceDataJNI.registerSimDeviceCreatedCallback(prefix, callback, initialNotify);
-    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceCreatedCallback);
-  }
-
-  public CallbackStore registerDeviceFreedCallback(String prefix, SimDeviceCallback callback) {
-    int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback);
-    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceFreedCallback);
-  }
-
-  public static void resetData() {
-    SimDeviceDataJNI.resetSimDeviceData();
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java b/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java
deleted file mode 100644
index 9805497..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.sim.mockdata.SimulatorJNI;
-
-public final class SimHooks {
-  private SimHooks() {
-  }
-
-  public static void waitForProgramStart() {
-    SimulatorJNI.waitForProgramStart();
-  }
-
-  public static void setProgramStarted() {
-    SimulatorJNI.setProgramStarted();
-  }
-
-  public static void restartTiming() {
-    SimulatorJNI.restartTiming();
-  }
-
-  public static void pauseTiming() {
-    SimulatorJNI.pauseTiming();
-  }
-
-  public static void resumeTiming() {
-    SimulatorJNI.resumeTiming();
-  }
-
-  public static boolean isTimingPaused() {
-    return SimulatorJNI.isTimingPaused();
-  }
-
-  public static void stepTiming(long delta) {
-    SimulatorJNI.stepTiming(delta);
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/SimValueCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SimValueCallback.java b/hal/src/main/java/edu/wpi/first/hal/sim/SimValueCallback.java
deleted file mode 100644
index 786158a..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SimValueCallback.java
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import edu.wpi.first.hal.HALValue;
-
-@FunctionalInterface
-public interface SimValueCallback {
-  void callback(String name, int handle, boolean readonly, HALValue value);
-
-  default void callbackNative(String name, int handle, boolean readonly, int type, long value1, double value2) {
-    callback(name, handle, readonly, HALValue.fromNative(type, value1, value2));
-  }
-}

hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java
deleted file mode 100644
index 5083fab..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback.java
+++ /dev/null
@@ -1,12 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-public interface SpiReadAutoReceiveBufferCallback {
-  int callback(String name, int[] buffer, int numToRead);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AccelerometerDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AccelerometerDataJNI.java
deleted file mode 100644
index 22276d4..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AccelerometerDataJNI.java
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class AccelerometerDataJNI extends JNIWrapper {
-  public static native int registerActiveCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelActiveCallback(int index, int uid);
-  public static native boolean getActive(int index);
-  public static native void setActive(int index, boolean active);
-
-  public static native int registerRangeCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelRangeCallback(int index, int uid);
-  public static native int getRange(int index);
-  public static native void setRange(int index, int range);
-
-  public static native int registerXCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelXCallback(int index, int uid);
-  public static native double getX(int index);
-  public static native void setX(int index, double x);
-
-  public static native int registerYCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelYCallback(int index, int uid);
-  public static native double getY(int index);
-  public static native void setY(int index, double y);
-
-  public static native int registerZCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelZCallback(int index, int uid);
-  public static native double getZ(int index);
-  public static native void setZ(int index, double z);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AddressableLEDDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AddressableLEDDataJNI.java
deleted file mode 100644
index 8098e68..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AddressableLEDDataJNI.java
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.ConstBufferCallback;
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class AddressableLEDDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerOutputPortCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelOutputPortCallback(int index, int uid);
-  public static native int getOutputPort(int index);
-  public static native void setOutputPort(int index, int outputPort);
-
-  public static native int registerLengthCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelLengthCallback(int index, int uid);
-  public static native int getLength(int index);
-  public static native void setLength(int index, int length);
-
-  public static native int registerRunningCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelRunningCallback(int index, int uid);
-  public static native boolean getRunning(int index);
-  public static native void setRunning(int index, boolean running);
-
-  public static native int registerDataCallback(int index, ConstBufferCallback callback);
-  public static native void cancelDataCallback(int index, int uid);
-  public static native byte[] getData(int index);
-  public static native void setData(int index, byte[] data);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogGyroDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogGyroDataJNI.java
deleted file mode 100644
index 4995df2..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogGyroDataJNI.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class AnalogGyroDataJNI extends JNIWrapper {
-  public static native int registerAngleCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAngleCallback(int index, int uid);
-  public static native double getAngle(int index);
-  public static native void setAngle(int index, double angle);
-
-  public static native int registerRateCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelRateCallback(int index, int uid);
-  public static native double getRate(int index);
-  public static native void setRate(int index, double rate);
-
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogInDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogInDataJNI.java
deleted file mode 100644
index ed4fbac..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogInDataJNI.java
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class AnalogInDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerAverageBitsCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAverageBitsCallback(int index, int uid);
-  public static native int getAverageBits(int index);
-  public static native void setAverageBits(int index, int averageBits);
-
-  public static native int registerOversampleBitsCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelOversampleBitsCallback(int index, int uid);
-  public static native int getOversampleBits(int index);
-  public static native void setOversampleBits(int index, int oversampleBits);
-
-  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelVoltageCallback(int index, int uid);
-  public static native double getVoltage(int index);
-  public static native void setVoltage(int index, double voltage);
-
-  public static native int registerAccumulatorInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAccumulatorInitializedCallback(int index, int uid);
-  public static native boolean getAccumulatorInitialized(int index);
-  public static native void setAccumulatorInitialized(int index, boolean accumulatorInitialized);
-
-  public static native int registerAccumulatorValueCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAccumulatorValueCallback(int index, int uid);
-  public static native long getAccumulatorValue(int index);
-  public static native void setAccumulatorValue(int index, long accumulatorValue);
-
-  public static native int registerAccumulatorCountCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAccumulatorCountCallback(int index, int uid);
-  public static native long getAccumulatorCount(int index);
-  public static native void setAccumulatorCount(int index, long accumulatorCount);
-
-  public static native int registerAccumulatorCenterCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAccumulatorCenterCallback(int index, int uid);
-  public static native int getAccumulatorCenter(int index);
-  public static native void setAccumulatorCenter(int index, int AccumulatorCenter);
-
-  public static native int registerAccumulatorDeadbandCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAccumulatorDeadbandCallback(int index, int uid);
-  public static native int getAccumulatorDeadband(int index);
-  public static native void setAccumulatorDeadband(int index, int AccumulatorDeadband);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogOutDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogOutDataJNI.java
deleted file mode 100644
index 2f7596c..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogOutDataJNI.java
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class AnalogOutDataJNI extends JNIWrapper {
-  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelVoltageCallback(int index, int uid);
-  public static native double getVoltage(int index);
-  public static native void setVoltage(int index, double voltage);
-
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogTriggerDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogTriggerDataJNI.java
deleted file mode 100644
index cf4187f..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/AnalogTriggerDataJNI.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class AnalogTriggerDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerTriggerLowerBoundCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelTriggerLowerBoundCallback(int index, int uid);
-  public static native double getTriggerLowerBound(int index);
-  public static native void setTriggerLowerBound(int index, double triggerLowerBound);
-
-  public static native int registerTriggerUpperBoundCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelTriggerUpperBoundCallback(int index, int uid);
-  public static native double getTriggerUpperBound(int index);
-  public static native void setTriggerUpperBound(int index, double triggerUpperBound);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DIODataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DIODataJNI.java
deleted file mode 100644
index 55de7de..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DIODataJNI.java
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class DIODataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerValueCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelValueCallback(int index, int uid);
-  public static native boolean getValue(int index);
-  public static native void setValue(int index, boolean value);
-
-  public static native int registerPulseLengthCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelPulseLengthCallback(int index, int uid);
-  public static native double getPulseLength(int index);
-  public static native void setPulseLength(int index, double pulseLength);
-
-  public static native int registerIsInputCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelIsInputCallback(int index, int uid);
-  public static native boolean getIsInput(int index);
-  public static native void setIsInput(int index, boolean isInput);
-
-  public static native int registerFilterIndexCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelFilterIndexCallback(int index, int uid);
-  public static native int getFilterIndex(int index);
-  public static native void setFilterIndex(int index, int filterIndex);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DigitalPWMDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DigitalPWMDataJNI.java
deleted file mode 100644
index 69bee07..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DigitalPWMDataJNI.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class DigitalPWMDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerDutyCycleCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelDutyCycleCallback(int index, int uid);
-  public static native double getDutyCycle(int index);
-  public static native void setDutyCycle(int index, double dutyCycle);
-
-  public static native int registerPinCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelPinCallback(int index, int uid);
-  public static native int getPin(int index);
-  public static native void setPin(int index, int pin);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.java
deleted file mode 100644
index ec19735..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DriverStationDataJNI.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.JNIWrapper;
-import edu.wpi.first.hal.sim.NotifyCallback;
-
-public class DriverStationDataJNI extends JNIWrapper {
-  public static native int registerEnabledCallback(NotifyCallback callback, boolean initialNotify);
-  public static native void cancelEnabledCallback(int uid);
-  public static native boolean getEnabled();
-  public static native void setEnabled(boolean enabled);
-
-  public static native int registerAutonomousCallback(NotifyCallback callback, boolean initialNotify);
-  public static native void cancelAutonomousCallback(int uid);
-  public static native boolean getAutonomous();
-  public static native void setAutonomous(boolean autonomous);
-
-  public static native int registerTestCallback(NotifyCallback callback, boolean initialNotify);
-  public static native void cancelTestCallback(int uid);
-  public static native boolean getTest();
-  public static native void setTest(boolean test);
-
-  public static native int registerEStopCallback(NotifyCallback callback, boolean initialNotify);
-  public static native void cancelEStopCallback(int uid);
-  public static native boolean getEStop();
-  public static native void setEStop(boolean eStop);
-
-  public static native int registerFmsAttachedCallback(NotifyCallback callback, boolean initialNotify);
-  public static native void cancelFmsAttachedCallback(int uid);
-  public static native boolean getFmsAttached();
-  public static native void setFmsAttached(boolean fmsAttached);
-
-  public static native int registerDsAttachedCallback(NotifyCallback callback, boolean initialNotify);
-  public static native void cancelDsAttachedCallback(int uid);
-  public static native boolean getDsAttached();
-  public static native void setDsAttached(boolean dsAttached);
-
-  public static native void setJoystickAxes(byte joystickNum, float[] axesArray);
-  public static native void setJoystickPOVs(byte joystickNum, short[] povsArray);
-  public static native void setJoystickButtons(byte joystickNum, int buttons, int count);
-
-  public static native void setMatchInfo(String eventName, String gameSpecificMessage, int matchNumber, int replayNumber, int matchType);
-  public static native void registerAllCallbacks(NotifyCallback callback, boolean initialNotify);
-  public static native void notifyNewData();
-
-  public static native void setSendError(boolean shouldSend);
-
-  public static native void resetData();
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DutyCycleDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DutyCycleDataJNI.java
deleted file mode 100644
index 5cbae6e..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/DutyCycleDataJNI.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class DutyCycleDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerFrequencyCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelFrequencyCallback(int index, int uid);
-  public static native int getFrequency(int index);
-  public static native void setFrequency(int index, int frequency);
-
-  public static native int registerOutputCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelOutputCallback(int index, int uid);
-  public static native double getOutput(int index);
-  public static native void setOutput(int index, double output);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/EncoderDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/EncoderDataJNI.java
deleted file mode 100644
index 17e242e..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/EncoderDataJNI.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class EncoderDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerCountCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCountCallback(int index, int uid);
-  public static native int getCount(int index);
-  public static native void setCount(int index, int count);
-
-  public static native int registerPeriodCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelPeriodCallback(int index, int uid);
-  public static native double getPeriod(int index);
-  public static native void setPeriod(int index, double period);
-
-  public static native int registerResetCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelResetCallback(int index, int uid);
-  public static native boolean getReset(int index);
-  public static native void setReset(int index, boolean reset);
-
-  public static native int registerMaxPeriodCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelMaxPeriodCallback(int index, int uid);
-  public static native double getMaxPeriod(int index);
-  public static native void setMaxPeriod(int index, double maxPeriod);
-
-  public static native int registerDirectionCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelDirectionCallback(int index, int uid);
-  public static native boolean getDirection(int index);
-  public static native void setDirection(int index, boolean direction);
-
-  public static native int registerReverseDirectionCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelReverseDirectionCallback(int index, int uid);
-  public static native boolean getReverseDirection(int index);
-  public static native void setReverseDirection(int index, boolean reverseDirection);
-
-  public static native int registerSamplesToAverageCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelSamplesToAverageCallback(int index, int uid);
-  public static native int getSamplesToAverage(int index);
-  public static native void setSamplesToAverage(int index, int samplesToAverage);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/I2CDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/I2CDataJNI.java
deleted file mode 100644
index 33d78f8..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/I2CDataJNI.java
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.BufferCallback;
-import edu.wpi.first.hal.sim.ConstBufferCallback;
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class I2CDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerReadCallback(int index, BufferCallback callback);
-  public static native void cancelReadCallback(int index, int uid);
-
-  public static native int registerWriteCallback(int index, ConstBufferCallback callback);
-  public static native void cancelWriteCallback(int index, int uid);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/NotifierDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/NotifierDataJNI.java
deleted file mode 100644
index ecc0842..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/NotifierDataJNI.java
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.JNIWrapper;
-
-public class NotifierDataJNI extends JNIWrapper {
-  public static native long getNextTimeout();
-  public static native int getNumNotifiers();
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PCMDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PCMDataJNI.java
deleted file mode 100644
index c5de287..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PCMDataJNI.java
+++ /dev/null
@@ -1,53 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class PCMDataJNI extends JNIWrapper {
-  public static native int registerSolenoidInitializedCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelSolenoidInitializedCallback(int index, int channel, int uid);
-  public static native boolean getSolenoidInitialized(int index, int channel);
-  public static native void setSolenoidInitialized(int index, int channel, boolean solenoidInitialized);
-
-  public static native int registerSolenoidOutputCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelSolenoidOutputCallback(int index, int channel, int uid);
-  public static native boolean getSolenoidOutput(int index, int channel);
-  public static native void setSolenoidOutput(int index, int channel, boolean solenoidOutput);
-
-  public static native int registerCompressorInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCompressorInitializedCallback(int index, int uid);
-  public static native boolean getCompressorInitialized(int index);
-  public static native void setCompressorInitialized(int index, boolean compressorInitialized);
-
-  public static native int registerCompressorOnCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCompressorOnCallback(int index, int uid);
-  public static native boolean getCompressorOn(int index);
-  public static native void setCompressorOn(int index, boolean compressorOn);
-
-  public static native int registerClosedLoopEnabledCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelClosedLoopEnabledCallback(int index, int uid);
-  public static native boolean getClosedLoopEnabled(int index);
-  public static native void setClosedLoopEnabled(int index, boolean closeLoopEnabled);
-
-  public static native int registerPressureSwitchCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelPressureSwitchCallback(int index, int uid);
-  public static native boolean getPressureSwitch(int index);
-  public static native void setPressureSwitch(int index, boolean pressureSwitch);
-
-  public static native int registerCompressorCurrentCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCompressorCurrentCallback(int index, int uid);
-  public static native double getCompressorCurrent(int index);
-  public static native void setCompressorCurrent(int index, double compressorCurrent);
-
-  public static native void registerAllNonSolenoidCallbacks(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void registerAllSolenoidCallbacks(int index, int channel, NotifyCallback callback, boolean initialNotify);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PDPDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PDPDataJNI.java
deleted file mode 100644
index 581a727..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PDPDataJNI.java
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class PDPDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerTemperatureCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelTemperatureCallback(int index, int uid);
-  public static native double getTemperature(int index);
-  public static native void setTemperature(int index, double temperature);
-
-  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelVoltageCallback(int index, int uid);
-  public static native double getVoltage(int index);
-  public static native void setVoltage(int index, double voltage);
-
-
-  public static native int registerCurrentCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCurrentCallback(int index, int channel, int uid);
-  public static native double getCurrent(int index, int channel);
-  public static native void setCurrent(int index, int channel, double current);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PWMDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PWMDataJNI.java
deleted file mode 100644
index e36990e..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/PWMDataJNI.java
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class PWMDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerRawValueCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelRawValueCallback(int index, int uid);
-  public static native int getRawValue(int index);
-  public static native void setRawValue(int index, int rawValue);
-
-  public static native int registerSpeedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelSpeedCallback(int index, int uid);
-  public static native double getSpeed(int index);
-  public static native void setSpeed(int index, double speed);
-
-  public static native int registerPositionCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelPositionCallback(int index, int uid);
-  public static native double getPosition(int index);
-  public static native void setPosition(int index, double position);
-
-  public static native int registerPeriodScaleCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelPeriodScaleCallback(int index, int uid);
-  public static native int getPeriodScale(int index);
-  public static native void setPeriodScale(int index, int periodScale);
-
-  public static native int registerZeroLatchCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelZeroLatchCallback(int index, int uid);
-  public static native boolean getZeroLatch(int index);
-  public static native void setZeroLatch(int index, boolean zeroLatch);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/RelayDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/RelayDataJNI.java
deleted file mode 100644
index 320a86d..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/RelayDataJNI.java
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class RelayDataJNI extends JNIWrapper {
-  public static native int registerInitializedForwardCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedForwardCallback(int index, int uid);
-  public static native boolean getInitializedForward(int index);
-  public static native void setInitializedForward(int index, boolean initializedForward);
-
-  public static native int registerInitializedReverseCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedReverseCallback(int index, int uid);
-  public static native boolean getInitializedReverse(int index);
-  public static native void setInitializedReverse(int index, boolean initializedReverse);
-
-  public static native int registerForwardCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelForwardCallback(int index, int uid);
-  public static native boolean getForward(int index);
-  public static native void setForward(int index, boolean forward);
-
-  public static native int registerReverseCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelReverseCallback(int index, int uid);
-  public static native boolean getReverse(int index);
-  public static native void setReverse(int index, boolean reverse);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/RoboRioDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/RoboRioDataJNI.java
deleted file mode 100644
index a13845c..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/RoboRioDataJNI.java
+++ /dev/null
@@ -1,90 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class RoboRioDataJNI extends JNIWrapper {
-  public static native int registerFPGAButtonCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelFPGAButtonCallback(int index, int uid);
-  public static native boolean getFPGAButton(int index);
-  public static native void setFPGAButton(int index, boolean fPGAButton);
-
-  public static native int registerVInVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelVInVoltageCallback(int index, int uid);
-  public static native double getVInVoltage(int index);
-  public static native void setVInVoltage(int index, double vInVoltage);
-
-  public static native int registerVInCurrentCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelVInCurrentCallback(int index, int uid);
-  public static native double getVInCurrent(int index);
-  public static native void setVInCurrent(int index, double vInCurrent);
-
-  public static native int registerUserVoltage6VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserVoltage6VCallback(int index, int uid);
-  public static native double getUserVoltage6V(int index);
-  public static native void setUserVoltage6V(int index, double userVoltage6V);
-
-  public static native int registerUserCurrent6VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserCurrent6VCallback(int index, int uid);
-  public static native double getUserCurrent6V(int index);
-  public static native void setUserCurrent6V(int index, double userCurrent6V);
-
-  public static native int registerUserActive6VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserActive6VCallback(int index, int uid);
-  public static native boolean getUserActive6V(int index);
-  public static native void setUserActive6V(int index, boolean userActive6V);
-
-  public static native int registerUserVoltage5VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserVoltage5VCallback(int index, int uid);
-  public static native double getUserVoltage5V(int index);
-  public static native void setUserVoltage5V(int index, double userVoltage5V);
-
-  public static native int registerUserCurrent5VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserCurrent5VCallback(int index, int uid);
-  public static native double getUserCurrent5V(int index);
-  public static native void setUserCurrent5V(int index, double userCurrent5V);
-
-  public static native int registerUserActive5VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserActive5VCallback(int index, int uid);
-  public static native boolean getUserActive5V(int index);
-  public static native void setUserActive5V(int index, boolean userActive5V);
-
-  public static native int registerUserVoltage3V3Callback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserVoltage3V3Callback(int index, int uid);
-  public static native double getUserVoltage3V3(int index);
-  public static native void setUserVoltage3V3(int index, double userVoltage3V3);
-
-  public static native int registerUserCurrent3V3Callback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserCurrent3V3Callback(int index, int uid);
-  public static native double getUserCurrent3V3(int index);
-  public static native void setUserCurrent3V3(int index, double userCurrent3V3);
-
-  public static native int registerUserActive3V3Callback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserActive3V3Callback(int index, int uid);
-  public static native boolean getUserActive3V3(int index);
-  public static native void setUserActive3V3(int index, boolean userActive3V3);
-
-  public static native int registerUserFaults6VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserFaults6VCallback(int index, int uid);
-  public static native int getUserFaults6V(int index);
-  public static native void setUserFaults6V(int index, int userFaults6V);
-
-  public static native int registerUserFaults5VCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserFaults5VCallback(int index, int uid);
-  public static native int getUserFaults5V(int index);
-  public static native void setUserFaults5V(int index, int userFaults5V);
-
-  public static native int registerUserFaults3V3Callback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelUserFaults3V3Callback(int index, int uid);
-  public static native int getUserFaults3V3(int index);
-  public static native void setUserFaults3V3(int index, int userFaults3V3);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SPIAccelerometerDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SPIAccelerometerDataJNI.java
deleted file mode 100644
index d8e7353..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SPIAccelerometerDataJNI.java
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class SPIAccelerometerDataJNI extends JNIWrapper {
-  public static native int registerActiveCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelActiveCallback(int index, int uid);
-  public static native boolean getActive(int index);
-  public static native void setActive(int index, boolean active);
-
-  public static native int registerRangeCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelRangeCallback(int index, int uid);
-  public static native int getRange(int index);
-  public static native void setRange(int index, int range);
-
-  public static native int registerXCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelXCallback(int index, int uid);
-  public static native double getX(int index);
-  public static native void setX(int index, double x);
-
-  public static native int registerYCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelYCallback(int index, int uid);
-  public static native double getY(int index);
-  public static native void setY(int index, double y);
-
-  public static native int registerZCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelZCallback(int index, int uid);
-  public static native double getZ(int index);
-  public static native void setZ(int index, double z);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SPIDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SPIDataJNI.java
deleted file mode 100644
index a12ec66..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SPIDataJNI.java
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.BufferCallback;
-import edu.wpi.first.hal.sim.ConstBufferCallback;
-import edu.wpi.first.hal.sim.NotifyCallback;
-import edu.wpi.first.hal.sim.SpiReadAutoReceiveBufferCallback;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class SPIDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerReadCallback(int index, BufferCallback callback);
-  public static native void cancelReadCallback(int index, int uid);
-
-  public static native int registerWriteCallback(int index, ConstBufferCallback callback);
-  public static native void cancelWriteCallback(int index, int uid);
-
-  public static native int registerReadAutoReceiveBufferCallback(int index, SpiReadAutoReceiveBufferCallback callback);
-  public static native void cancelReadAutoReceiveBufferCallback(int index, int uid);
-
-  public static native void resetData(int index);
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimDeviceDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimDeviceDataJNI.java
deleted file mode 100644
index 2b8a38a..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimDeviceDataJNI.java
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.sim.SimDeviceCallback;
-import edu.wpi.first.hal.sim.SimValueCallback;
-import edu.wpi.first.hal.HALValue;
-import edu.wpi.first.hal.JNIWrapper;
-
-public class SimDeviceDataJNI extends JNIWrapper {
-  public static native int registerSimDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify);
-  public static native void cancelSimDeviceCreatedCallback(int uid);
-
-  public static native int registerSimDeviceFreedCallback(String prefix, SimDeviceCallback callback);
-  public static native void cancelSimDeviceFreedCallback(int uid);
-
-  public static native int getSimDeviceHandle(String name);
-
-  public static native int getSimValueDeviceHandle(int handle);
-
-  public static class SimDeviceInfo {
-    public SimDeviceInfo(String name, int handle) {
-      this.name = name;
-      this.handle = handle;
-    }
-
-    @SuppressWarnings("MemberName")
-    String name;
-
-    @SuppressWarnings("MemberName")
-    int handle;
-  }
-  public static native SimDeviceInfo[] enumerateSimDevices(String prefix);
-
-  public static native int registerSimValueCreatedCallback(int device, SimValueCallback callback, boolean initialNotify);
-  public static native void cancelSimValueCreatedCallback(int uid);
-
-  public static native int registerSimValueChangedCallback(int handle, SimValueCallback callback, boolean initialNotify);
-  public static native void cancelSimValueChangedCallback(int uid);
-
-  public static native int getSimValueHandle(int device, String name);
-
-  public static class SimValueInfo {
-    public SimValueInfo(String name, int handle, boolean readonly, int type, long value1, double value2) {
-      this.name = name;
-      this.handle = handle;
-      this.readonly = readonly;
-      this.value = HALValue.fromNative(type, value1, value2);
-    }
-
-    @SuppressWarnings("MemberName")
-    String name;
-
-    @SuppressWarnings("MemberName")
-    int handle;
-
-    @SuppressWarnings("MemberName")
-    boolean readonly;
-
-    @SuppressWarnings("MemberName")
-    HALValue value;
-  }
-  public static native SimValueInfo[] enumerateSimValues(int device);
-
-  public static native String[] getSimValueEnumOptions(int handle);
-
-  public static native void resetSimDeviceData();
-}

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

diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java
deleted file mode 100644
index bfd8505..0000000
--- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim.mockdata;
-
-import edu.wpi.first.hal.JNIWrapper;
-
-public class SimulatorJNI extends JNIWrapper {
-  public static native void waitForProgramStart();
-  public static native void setProgramStarted();
-  public static native void restartTiming();
-  public static native void pauseTiming();
-  public static native void resumeTiming();
-  public static native boolean isTimingPaused();
-  public static native void stepTiming(long delta);
-  public static native void resetHandles();
-}

hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
new file mode 100644
index 0000000..cb42c98
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class AccelerometerDataJNI extends JNIWrapper {
+  public static native int registerActiveCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelActiveCallback(int index, int uid);
+  public static native boolean getActive(int index);
+  public static native void setActive(int index, boolean active);
+
+  public static native int registerRangeCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelRangeCallback(int index, int uid);
+  public static native int getRange(int index);
+  public static native void setRange(int index, int range);
+
+  public static native int registerXCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelXCallback(int index, int uid);
+  public static native double getX(int index);
+  public static native void setX(int index, double x);
+
+  public static native int registerYCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelYCallback(int index, int uid);
+  public static native double getY(int index);
+  public static native void setY(int index, double y);
+
+  public static native int registerZCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelZCallback(int index, int uid);
+  public static native double getZ(int index);
+  public static native void setZ(int index, double z);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
new file mode 100644
index 0000000..0aa4d47
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class AddressableLEDDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerOutputPortCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelOutputPortCallback(int index, int uid);
+  public static native int getOutputPort(int index);
+  public static native void setOutputPort(int index, int outputPort);
+
+  public static native int registerLengthCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelLengthCallback(int index, int uid);
+  public static native int getLength(int index);
+  public static native void setLength(int index, int length);
+
+  public static native int registerRunningCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelRunningCallback(int index, int uid);
+  public static native boolean getRunning(int index);
+  public static native void setRunning(int index, boolean running);
+
+  public static native int registerDataCallback(int index, ConstBufferCallback callback);
+  public static native void cancelDataCallback(int index, int uid);
+  public static native byte[] getData(int index);
+  public static native void setData(int index, byte[] data);
+
+  public static native void resetData(int index);
+
+  public static native int findForChannel(int channel);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
new file mode 100644
index 0000000..42cc508
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class AnalogGyroDataJNI extends JNIWrapper {
+  public static native int registerAngleCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAngleCallback(int index, int uid);
+  public static native double getAngle(int index);
+  public static native void setAngle(int index, double angle);
+
+  public static native int registerRateCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelRateCallback(int index, int uid);
+  public static native double getRate(int index);
+  public static native void setRate(int index, double rate);
+
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
new file mode 100644
index 0000000..3656ffa
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class AnalogInDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerAverageBitsCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAverageBitsCallback(int index, int uid);
+  public static native int getAverageBits(int index);
+  public static native void setAverageBits(int index, int averageBits);
+
+  public static native int registerOversampleBitsCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelOversampleBitsCallback(int index, int uid);
+  public static native int getOversampleBits(int index);
+  public static native void setOversampleBits(int index, int oversampleBits);
+
+  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelVoltageCallback(int index, int uid);
+  public static native double getVoltage(int index);
+  public static native void setVoltage(int index, double voltage);
+
+  public static native int registerAccumulatorInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAccumulatorInitializedCallback(int index, int uid);
+  public static native boolean getAccumulatorInitialized(int index);
+  public static native void setAccumulatorInitialized(int index, boolean accumulatorInitialized);
+
+  public static native int registerAccumulatorValueCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAccumulatorValueCallback(int index, int uid);
+  public static native long getAccumulatorValue(int index);
+  public static native void setAccumulatorValue(int index, long accumulatorValue);
+
+  public static native int registerAccumulatorCountCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAccumulatorCountCallback(int index, int uid);
+  public static native long getAccumulatorCount(int index);
+  public static native void setAccumulatorCount(int index, long accumulatorCount);
+
+  public static native int registerAccumulatorCenterCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAccumulatorCenterCallback(int index, int uid);
+  public static native int getAccumulatorCenter(int index);
+  public static native void setAccumulatorCenter(int index, int AccumulatorCenter);
+
+  public static native int registerAccumulatorDeadbandCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAccumulatorDeadbandCallback(int index, int uid);
+  public static native int getAccumulatorDeadband(int index);
+  public static native void setAccumulatorDeadband(int index, int AccumulatorDeadband);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
new file mode 100644
index 0000000..23bc8ac
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class AnalogOutDataJNI extends JNIWrapper {
+  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelVoltageCallback(int index, int uid);
+  public static native double getVoltage(int index);
+  public static native void setVoltage(int index, double voltage);
+
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
new file mode 100644
index 0000000..ca89a28
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class AnalogTriggerDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerTriggerLowerBoundCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelTriggerLowerBoundCallback(int index, int uid);
+  public static native double getTriggerLowerBound(int index);
+  public static native void setTriggerLowerBound(int index, double triggerLowerBound);
+
+  public static native int registerTriggerUpperBoundCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelTriggerUpperBoundCallback(int index, int uid);
+  public static native double getTriggerUpperBound(int index);
+  public static native void setTriggerUpperBound(int index, double triggerUpperBound);
+
+  public static native void resetData(int index);
+
+  public static native int findForChannel(int channel);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
new file mode 100644
index 0000000..a8d8ce1
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+public interface BufferCallback {
+  void callback(String name, byte[] buffer, int count);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java
new file mode 100644
index 0000000..6cb2375
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+public interface ConstBufferCallback {
+  void callback(String name, byte[] buffer, int count);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
new file mode 100644
index 0000000..5d41a2f
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class DIODataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerValueCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelValueCallback(int index, int uid);
+  public static native boolean getValue(int index);
+  public static native void setValue(int index, boolean value);
+
+  public static native int registerPulseLengthCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelPulseLengthCallback(int index, int uid);
+  public static native double getPulseLength(int index);
+  public static native void setPulseLength(int index, double pulseLength);
+
+  public static native int registerIsInputCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelIsInputCallback(int index, int uid);
+  public static native boolean getIsInput(int index);
+  public static native void setIsInput(int index, boolean isInput);
+
+  public static native int registerFilterIndexCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelFilterIndexCallback(int index, int uid);
+  public static native int getFilterIndex(int index);
+  public static native void setFilterIndex(int index, int filterIndex);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
new file mode 100644
index 0000000..4e2709f
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class DigitalPWMDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerDutyCycleCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelDutyCycleCallback(int index, int uid);
+  public static native double getDutyCycle(int index);
+  public static native void setDutyCycle(int index, double dutyCycle);
+
+  public static native int registerPinCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelPinCallback(int index, int uid);
+  public static native int getPin(int index);
+  public static native void setPin(int index, int pin);
+
+  public static native void resetData(int index);
+
+  public static native int findForChannel(int channel);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
new file mode 100644
index 0000000..a1046c5
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class DriverStationDataJNI extends JNIWrapper {
+  public static native int registerEnabledCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelEnabledCallback(int uid);
+  public static native boolean getEnabled();
+  public static native void setEnabled(boolean enabled);
+
+  public static native int registerAutonomousCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAutonomousCallback(int uid);
+  public static native boolean getAutonomous();
+  public static native void setAutonomous(boolean autonomous);
+
+  public static native int registerTestCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelTestCallback(int uid);
+  public static native boolean getTest();
+  public static native void setTest(boolean test);
+
+  public static native int registerEStopCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelEStopCallback(int uid);
+  public static native boolean getEStop();
+  public static native void setEStop(boolean eStop);
+
+  public static native int registerFmsAttachedCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelFmsAttachedCallback(int uid);
+  public static native boolean getFmsAttached();
+  public static native void setFmsAttached(boolean fmsAttached);
+
+  public static native int registerDsAttachedCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelDsAttachedCallback(int uid);
+  public static native boolean getDsAttached();
+  public static native void setDsAttached(boolean dsAttached);
+
+  public static native int registerAllianceStationIdCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelAllianceStationIdCallback(int uid);
+  public static native int getAllianceStationId();
+  public static native void setAllianceStationId(int allianceStationId);
+
+  public static native int registerMatchTimeCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelMatchTimeCallback(int uid);
+  public static native double getMatchTime();
+  public static native void setMatchTime(double matchTime);
+
+  public static native void setJoystickAxes(byte joystickNum, float[] axesArray);
+  public static native void setJoystickPOVs(byte joystickNum, short[] povsArray);
+  public static native void setJoystickButtons(byte joystickNum, int buttons, int count);
+  public static native long getJoystickOutputs(int stick);
+  public static native int getJoystickRumble(int stick, int rumbleNum);
+
+  public static native void setMatchInfo(String eventName, String gameSpecificMessage, int matchNumber, int replayNumber, int matchType);
+
+  public static native void registerAllCallbacks(NotifyCallback callback, boolean initialNotify);
+  public static native void notifyNewData();
+
+  public static native void setSendError(boolean shouldSend);
+  public static native void setSendConsoleLine(boolean shouldSend);
+
+  public static native void setJoystickButton(int stick, int button, boolean state);
+  public static native void setJoystickAxis(int stick, int axis, double value);
+  public static native void setJoystickPOV(int stick, int pov, int value);
+  public static native void setJoystickButtonsValue(int stick, int buttons);
+  public static native void setJoystickAxisCount(int stick, int count);
+  public static native void setJoystickPOVCount(int stick, int count);
+  public static native void setJoystickButtonCount(int stick, int count);
+
+  public static native void setJoystickIsXbox(int stick, boolean isXbox);
+  public static native void setJoystickType(int stick, int type);
+  public static native void setJoystickName(int stick, String name);
+  public static native void setJoystickAxisType(int stick, int axis, int type);
+
+  public static native void setGameSpecificMessage(String message);
+  public static native void setEventName(String name);
+  public static native void setMatchType(int type);
+  public static native void setMatchNumber(int matchNumber);
+  public static native void setReplayNumber(int replayNumber);
+
+  public static native void resetData();
+}

hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
new file mode 100644
index 0000000..3228341
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class DutyCycleDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerFrequencyCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelFrequencyCallback(int index, int uid);
+  public static native int getFrequency(int index);
+  public static native void setFrequency(int index, int frequency);
+
+  public static native int registerOutputCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelOutputCallback(int index, int uid);
+  public static native double getOutput(int index);
+  public static native void setOutput(int index, double output);
+
+  public static native void resetData(int index);
+
+  public static native int findForChannel(int channel);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
new file mode 100644
index 0000000..db78d3c
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class EncoderDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerCountCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelCountCallback(int index, int uid);
+  public static native int getCount(int index);
+  public static native void setCount(int index, int count);
+
+  public static native int registerPeriodCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelPeriodCallback(int index, int uid);
+  public static native double getPeriod(int index);
+  public static native void setPeriod(int index, double period);
+
+  public static native int registerResetCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelResetCallback(int index, int uid);
+  public static native boolean getReset(int index);
+  public static native void setReset(int index, boolean reset);
+
+  public static native int registerMaxPeriodCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelMaxPeriodCallback(int index, int uid);
+  public static native double getMaxPeriod(int index);
+  public static native void setMaxPeriod(int index, double maxPeriod);
+
+  public static native int registerDirectionCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelDirectionCallback(int index, int uid);
+  public static native boolean getDirection(int index);
+  public static native void setDirection(int index, boolean direction);
+
+  public static native int registerReverseDirectionCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelReverseDirectionCallback(int index, int uid);
+  public static native boolean getReverseDirection(int index);
+  public static native void setReverseDirection(int index, boolean reverseDirection);
+
+  public static native int registerSamplesToAverageCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelSamplesToAverageCallback(int index, int uid);
+  public static native int getSamplesToAverage(int index);
+  public static native void setSamplesToAverage(int index, int samplesToAverage);
+
+  public static native void setDistance(int index, double distance);
+  public static native double getDistance(int index);
+  public static native void setRate(int index, double rate);
+  public static native double getRate(int index);
+
+  public static native void resetData(int index);
+
+  public static native int findForChannel(int channel);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
new file mode 100644
index 0000000..8dbc9e6
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class I2CDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerReadCallback(int index, BufferCallback callback);
+  public static native void cancelReadCallback(int index, int uid);
+
+  public static native int registerWriteCallback(int index, ConstBufferCallback callback);
+  public static native void cancelWriteCallback(int index, int uid);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
new file mode 100644
index 0000000..823318c
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class NotifierDataJNI extends JNIWrapper {
+  public static native long getNextTimeout();
+  public static native int getNumNotifiers();
+}

hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java b/hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java
new file mode 100644
index 0000000..22ec015
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.HALValue;
+
+public interface NotifyCallback {
+  void callback(String name, HALValue value);
+
+  default void callbackNative(String name, int type, long value1, double value2) {
+    callback(name, HALValue.fromNative(type, value1, value2));
+  }
+}

hal/src/main/java/edu/wpi/first/hal/simulation/PCMDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/PCMDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/PCMDataJNI.java
new file mode 100644
index 0000000..6b72297
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/PCMDataJNI.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class PCMDataJNI extends JNIWrapper {
+  public static native int registerSolenoidInitializedCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelSolenoidInitializedCallback(int index, int channel, int uid);
+  public static native boolean getSolenoidInitialized(int index, int channel);
+  public static native void setSolenoidInitialized(int index, int channel, boolean solenoidInitialized);
+
+  public static native int registerSolenoidOutputCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelSolenoidOutputCallback(int index, int channel, int uid);
+  public static native boolean getSolenoidOutput(int index, int channel);
+  public static native void setSolenoidOutput(int index, int channel, boolean solenoidOutput);
+
+  public static native int registerCompressorInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelCompressorInitializedCallback(int index, int uid);
+  public static native boolean getCompressorInitialized(int index);
+  public static native void setCompressorInitialized(int index, boolean compressorInitialized);
+
+  public static native int registerCompressorOnCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelCompressorOnCallback(int index, int uid);
+  public static native boolean getCompressorOn(int index);
+  public static native void setCompressorOn(int index, boolean compressorOn);
+
+  public static native int registerClosedLoopEnabledCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelClosedLoopEnabledCallback(int index, int uid);
+  public static native boolean getClosedLoopEnabled(int index);
+  public static native void setClosedLoopEnabled(int index, boolean closeLoopEnabled);
+
+  public static native int registerPressureSwitchCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelPressureSwitchCallback(int index, int uid);
+  public static native boolean getPressureSwitch(int index);
+  public static native void setPressureSwitch(int index, boolean pressureSwitch);
+
+  public static native int registerCompressorCurrentCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelCompressorCurrentCallback(int index, int uid);
+  public static native double getCompressorCurrent(int index);
+  public static native void setCompressorCurrent(int index, double compressorCurrent);
+
+  public static native void registerAllNonSolenoidCallbacks(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void registerAllSolenoidCallbacks(int index, int channel, NotifyCallback callback, boolean initialNotify);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/PDPDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/PDPDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/PDPDataJNI.java
new file mode 100644
index 0000000..8825f60
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/PDPDataJNI.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class PDPDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerTemperatureCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelTemperatureCallback(int index, int uid);
+  public static native double getTemperature(int index);
+  public static native void setTemperature(int index, double temperature);
+
+  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelVoltageCallback(int index, int uid);
+  public static native double getVoltage(int index);
+  public static native void setVoltage(int index, double voltage);
+
+
+  public static native int registerCurrentCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelCurrentCallback(int index, int channel, int uid);
+  public static native double getCurrent(int index, int channel);
+  public static native void setCurrent(int index, int channel, double current);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
new file mode 100644
index 0000000..f44b56b
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class PWMDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerRawValueCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelRawValueCallback(int index, int uid);
+  public static native int getRawValue(int index);
+  public static native void setRawValue(int index, int rawValue);
+
+  public static native int registerSpeedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelSpeedCallback(int index, int uid);
+  public static native double getSpeed(int index);
+  public static native void setSpeed(int index, double speed);
+
+  public static native int registerPositionCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelPositionCallback(int index, int uid);
+  public static native double getPosition(int index);
+  public static native void setPosition(int index, double position);
+
+  public static native int registerPeriodScaleCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelPeriodScaleCallback(int index, int uid);
+  public static native int getPeriodScale(int index);
+  public static native void setPeriodScale(int index, int periodScale);
+
+  public static native int registerZeroLatchCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelZeroLatchCallback(int index, int uid);
+  public static native boolean getZeroLatch(int index);
+  public static native void setZeroLatch(int index, boolean zeroLatch);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
new file mode 100644
index 0000000..e7973a3
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class RelayDataJNI extends JNIWrapper {
+  public static native int registerInitializedForwardCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedForwardCallback(int index, int uid);
+  public static native boolean getInitializedForward(int index);
+  public static native void setInitializedForward(int index, boolean initializedForward);
+
+  public static native int registerInitializedReverseCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedReverseCallback(int index, int uid);
+  public static native boolean getInitializedReverse(int index);
+  public static native void setInitializedReverse(int index, boolean initializedReverse);
+
+  public static native int registerForwardCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelForwardCallback(int index, int uid);
+  public static native boolean getForward(int index);
+  public static native void setForward(int index, boolean forward);
+
+  public static native int registerReverseCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelReverseCallback(int index, int uid);
+  public static native boolean getReverse(int index);
+  public static native void setReverse(int index, boolean reverse);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
new file mode 100644
index 0000000..a95abfe
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class RoboRioDataJNI extends JNIWrapper {
+  public static native int registerFPGAButtonCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelFPGAButtonCallback(int uid);
+  public static native boolean getFPGAButton();
+  public static native void setFPGAButton(boolean fPGAButton);
+
+  public static native int registerVInVoltageCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelVInVoltageCallback(int uid);
+  public static native double getVInVoltage();
+  public static native void setVInVoltage(double vInVoltage);
+
+  public static native int registerVInCurrentCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelVInCurrentCallback(int uid);
+  public static native double getVInCurrent();
+  public static native void setVInCurrent(double vInCurrent);
+
+  public static native int registerUserVoltage6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserVoltage6VCallback(int uid);
+  public static native double getUserVoltage6V();
+  public static native void setUserVoltage6V(double userVoltage6V);
+
+  public static native int registerUserCurrent6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserCurrent6VCallback(int uid);
+  public static native double getUserCurrent6V();
+  public static native void setUserCurrent6V(double userCurrent6V);
+
+  public static native int registerUserActive6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserActive6VCallback(int uid);
+  public static native boolean getUserActive6V();
+  public static native void setUserActive6V(boolean userActive6V);
+
+  public static native int registerUserVoltage5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserVoltage5VCallback(int uid);
+  public static native double getUserVoltage5V();
+  public static native void setUserVoltage5V(double userVoltage5V);
+
+  public static native int registerUserCurrent5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserCurrent5VCallback(int uid);
+  public static native double getUserCurrent5V();
+  public static native void setUserCurrent5V(double userCurrent5V);
+
+  public static native int registerUserActive5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserActive5VCallback(int uid);
+  public static native boolean getUserActive5V();
+  public static native void setUserActive5V(boolean userActive5V);
+
+  public static native int registerUserVoltage3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserVoltage3V3Callback(int uid);
+  public static native double getUserVoltage3V3();
+  public static native void setUserVoltage3V3(double userVoltage3V3);
+
+  public static native int registerUserCurrent3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserCurrent3V3Callback(int uid);
+  public static native double getUserCurrent3V3();
+  public static native void setUserCurrent3V3(double userCurrent3V3);
+
+  public static native int registerUserActive3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserActive3V3Callback(int uid);
+  public static native boolean getUserActive3V3();
+  public static native void setUserActive3V3(boolean userActive3V3);
+
+  public static native int registerUserFaults6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserFaults6VCallback(int uid);
+  public static native int getUserFaults6V();
+  public static native void setUserFaults6V(int userFaults6V);
+
+  public static native int registerUserFaults5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserFaults5VCallback(int uid);
+  public static native int getUserFaults5V();
+  public static native void setUserFaults5V(int userFaults5V);
+
+  public static native int registerUserFaults3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native void cancelUserFaults3V3Callback(int uid);
+  public static native int getUserFaults3V3();
+  public static native void setUserFaults3V3(int userFaults3V3);
+
+  public static native void resetData();
+}

hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
new file mode 100644
index 0000000..2e8ebae
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class SPIAccelerometerDataJNI extends JNIWrapper {
+  public static native int registerActiveCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelActiveCallback(int index, int uid);
+  public static native boolean getActive(int index);
+  public static native void setActive(int index, boolean active);
+
+  public static native int registerRangeCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelRangeCallback(int index, int uid);
+  public static native int getRange(int index);
+  public static native void setRange(int index, int range);
+
+  public static native int registerXCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelXCallback(int index, int uid);
+  public static native double getX(int index);
+  public static native void setX(int index, double x);
+
+  public static native int registerYCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelYCallback(int index, int uid);
+  public static native double getY(int index);
+  public static native void setY(int index, double y);
+
+  public static native int registerZCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelZCallback(int index, int uid);
+  public static native double getZ(int index);
+  public static native void setZ(int index, double z);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
new file mode 100644
index 0000000..fd6b854
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class SPIDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native void cancelInitializedCallback(int index, int uid);
+  public static native boolean getInitialized(int index);
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerReadCallback(int index, BufferCallback callback);
+  public static native void cancelReadCallback(int index, int uid);
+
+  public static native int registerWriteCallback(int index, ConstBufferCallback callback);
+  public static native void cancelWriteCallback(int index, int uid);
+
+  public static native int registerReadAutoReceiveBufferCallback(int index, SpiReadAutoReceiveBufferCallback callback);
+  public static native void cancelReadAutoReceiveBufferCallback(int index, int uid);
+
+  public static native void resetData(int index);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java
new file mode 100644
index 0000000..2390bf4
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+@FunctionalInterface
+public interface SimDeviceCallback {
+  void callback(String name, int handle);
+}

hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
new file mode 100644
index 0000000..ea2a9f6
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.HALValue;
+import edu.wpi.first.hal.JNIWrapper;
+
+public class SimDeviceDataJNI extends JNIWrapper {
+  public static native void setSimDeviceEnabled(String prefix, boolean enabled);
+  public static native boolean isSimDeviceEnabled(String name);
+
+  public static native int registerSimDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify);
+  public static native void cancelSimDeviceCreatedCallback(int uid);
+
+  public static native int registerSimDeviceFreedCallback(String prefix, SimDeviceCallback callback);
+  public static native void cancelSimDeviceFreedCallback(int uid);
+
+  public static native int getSimDeviceHandle(String name);
+
+  public static native int getSimValueDeviceHandle(int handle);
+
+  public static class SimDeviceInfo {
+    public SimDeviceInfo(String name, int handle) {
+      this.name = name;
+      this.handle = handle;
+    }
+
+    @SuppressWarnings("MemberName")
+    public String name;
+
+    @SuppressWarnings("MemberName")
+    public int handle;
+  }
+  public static native SimDeviceInfo[] enumerateSimDevices(String prefix);
+
+  public static native int registerSimValueCreatedCallback(int device, SimValueCallback callback, boolean initialNotify);
+  public static native void cancelSimValueCreatedCallback(int uid);
+
+  public static native int registerSimValueChangedCallback(int handle, SimValueCallback callback, boolean initialNotify);
+  public static native void cancelSimValueChangedCallback(int uid);
+
+  public static native int getSimValueHandle(int device, String name);
+
+  public static class SimValueInfo {
+    public SimValueInfo(String name, int handle, boolean readonly, int type, long value1, double value2) {
+      this.name = name;
+      this.handle = handle;
+      this.readonly = readonly;
+      this.value = HALValue.fromNative(type, value1, value2);
+    }
+
+    @SuppressWarnings("MemberName")
+    public String name;
+
+    @SuppressWarnings("MemberName")
+    public int handle;
+
+    @SuppressWarnings("MemberName")
+    public boolean readonly;
+
+    @SuppressWarnings("MemberName")
+    public HALValue value;
+  }
+  public static native SimValueInfo[] enumerateSimValues(int device);
+
+  public static native String[] getSimValueEnumOptions(int handle);
+
+  public static native void resetSimDeviceData();
+}

hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java
new file mode 100644
index 0000000..d65ab38
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.HALValue;
+
+@FunctionalInterface
+public interface SimValueCallback {
+  void callback(String name, int handle, boolean readonly, HALValue value);
+
+  default void callbackNative(String name, int handle, boolean readonly, int type, long value1, double value2) {
+    callback(name, handle, readonly, HALValue.fromNative(type, value1, value2));
+  }
+}

hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
new file mode 100644
index 0000000..8147507
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class SimulatorJNI extends JNIWrapper {
+  public static native void setRuntimeType(int type);
+  public static native void waitForProgramStart();
+  public static native void setProgramStarted();
+  public static native boolean getProgramStarted();
+  public static native void restartTiming();
+  public static native void pauseTiming();
+  public static native void resumeTiming();
+  public static native boolean isTimingPaused();
+  public static native void stepTiming(long delta);
+  public static native void stepTimingAsync(long delta);
+  public static native void resetHandles();
+}

hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java
new file mode 100644
index 0000000..8061b5b
--- /dev/null
+++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.hal.simulation;
+
+public interface SpiReadAutoReceiveBufferCallback {
+  int callback(String name, int[] buffer, int numToRead);
+}

hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java b/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java
index 6f789d8..42e2059 100644
--- a/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java
+++ b/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,7 @@
 /**
  * Exception indicating that the resource is already allocated.
  */
+@SuppressWarnings("serial")
 public class AllocationException extends RuntimeException {
   /**
    * Create a new AllocationException.

hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java b/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java
index 8f21e60..8fef4c9 100644
--- a/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java
+++ b/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,7 @@
 /**
  * This exception represents an error in which a lower limit was set as higher than an upper limit.
  */
+@SuppressWarnings("serial")
 public class BoundaryException extends RuntimeException {
   /**
    * Create a new exception with the given message.

hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java b/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java
index f17e381..d9a8a80 100644
--- a/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java
+++ b/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,7 @@
  * Exception indicating that the resource is already allocated This is meant to be thrown by the
  * resource class.
  */
+@SuppressWarnings("serial")
 public class CheckedAllocationException extends Exception {
   /**
    * Create a new CheckedAllocationException.

hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java b/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java
index 874b07a..a775ec4 100644
--- a/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java
+++ b/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,8 +8,9 @@
 package edu.wpi.first.hal.util;
 
 /**
- * Exception indicating that an error has occured with a HAL Handle.
+ * Exception indicating that an error has occurred with a HAL Handle.
  */
+@SuppressWarnings("serial")
 public class HalHandleException extends RuntimeException {
   /**
    * Create a new HalHandleException.

hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java

diff --git a/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java b/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
index 3a3e217..3d6ab36 100644
--- a/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
+++ b/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,7 @@
 /**
  * Exception for bad status codes from the chip object.
  */
+@SuppressWarnings("serial")
 public final class UncleanStatusException extends IllegalStateException {
   private final int m_statusCode;
 

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

diff --git a/hal/src/main/native/athena/Accelerometer.cpp b/hal/src/main/native/athena/Accelerometer.cpp
index f83c06e..bc4d502 100644
--- a/hal/src/main/native/athena/Accelerometer.cpp
+++ b/hal/src/main/native/athena/Accelerometer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -176,7 +176,7 @@
  * 1 g-force, taking into account the accelerometer range.
  */
 static double unpackAxis(int16_t raw) {
-  // The raw value is actually 12 bits, not 16, so we need to propogate the
+  // The raw value is actually 12 bits, not 16, so we need to propagate the
   // 2's complement sign bit to the unused 4 bits for this to work with
   // negative numbers.
   raw <<= 4;

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

diff --git a/hal/src/main/native/athena/AddressableLED.cpp b/hal/src/main/native/athena/AddressableLED.cpp
index 64b6457..7334c99 100644
--- a/hal/src/main/native/athena/AddressableLED.cpp
+++ b/hal/src/main/native/athena/AddressableLED.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,7 @@
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
 #include "PortsInternal.h"
+#include "hal/AddressableLEDTypes.h"
 #include "hal/ChipObject.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"

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

diff --git a/hal/src/main/native/athena/AnalogInternal.cpp b/hal/src/main/native/athena/AnalogInternal.cpp
index 03a246d..6ac16b8 100644
--- a/hal/src/main/native/athena/AnalogInternal.cpp
+++ b/hal/src/main/native/athena/AnalogInternal.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -93,7 +93,7 @@
   config.ConvertRate = ticksPerConversion;
   analogInputSystem->writeConfig(config, status);
 
-  // Indicate that the scan size has been commited to hardware.
+  // Indicate that the scan size has been committed to hardware.
   setAnalogNumChannelsToActivate(0);
 }
 

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

diff --git a/hal/src/main/native/athena/CANAPI.cpp b/hal/src/main/native/athena/CANAPI.cpp
index 44cdb58..d460885 100644
--- a/hal/src/main/native/athena/CANAPI.cpp
+++ b/hal/src/main/native/athena/CANAPI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,6 @@
 
 #include "hal/CANAPI.h"
 
-#include <atomic>
 #include <ctime>
 
 #include <wpi/DenseMap.h>
@@ -15,7 +14,6 @@
 #include "HALInitializer.h"
 #include "hal/CAN.h"
 #include "hal/Errors.h"
-#include "hal/HAL.h"
 #include "hal/handles/UnlimitedHandleResource.h"
 
 using namespace hal;

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

diff --git a/hal/src/main/native/athena/DMA.cpp b/hal/src/main/native/athena/DMA.cpp
index 19c3068..ee39d2d 100644
--- a/hal/src/main/native/athena/DMA.cpp
+++ b/hal/src/main/native/athena/DMA.cpp
@@ -18,7 +18,7 @@
 #include "EncoderInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
-//#include "hal/AnalogGyro.h"
+#include "hal/AnalogGyro.h"
 #include "hal/AnalogInput.h"
 #include "hal/ChipObject.h"
 #include "hal/Errors.h"

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

diff --git a/hal/src/main/native/athena/DutyCycle.cpp b/hal/src/main/native/athena/DutyCycle.cpp
index 1c1a678..3749a2b 100644
--- a/hal/src/main/native/athena/DutyCycle.cpp
+++ b/hal/src/main/native/athena/DutyCycle.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -75,6 +75,9 @@
   dutyCycleHandles->Free(dutyCycleHandle);
 }
 
+void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
+                               HAL_SimDeviceHandle device) {}
+
 int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
                                   int32_t* status) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);

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

diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp
index 5b29815..51fe327 100644
--- a/hal/src/main/native/athena/FRCDriverStation.cpp
+++ b/hal/src/main/native/athena/FRCDriverStation.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -124,7 +124,7 @@
 
 static wpi::mutex* newDSDataAvailableMutex;
 static wpi::condition_variable* newDSDataAvailableCond;
-static std::atomic_int newDSDataAvailableCounter{0};
+static int newDSDataAvailableCounter{0};
 
 namespace hal {
 namespace init {
@@ -176,27 +176,27 @@
 
     if (baseLength + detailsRef.size() + locationRef.size() +
             callStackRef.size() <=
-        65536) {
+        65535) {
       // Pass through
       retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
                                                   details, location, callStack);
-    } else if (baseLength + detailsRef.size() > 65536) {
+    } else if (baseLength + detailsRef.size() > 65535) {
       // Details too long, cut both location and stack
-      auto newLen = 65536 - baseLength;
+      auto newLen = 65535 - baseLength;
       std::string newDetails{details, newLen};
       char empty = '\0';
       retval = FRC_NetworkCommunication_sendError(
           isError, errorCode, isLVCode, newDetails.c_str(), &empty, &empty);
-    } else if (baseLength + detailsRef.size() + locationRef.size() > 65536) {
+    } else if (baseLength + detailsRef.size() + locationRef.size() > 65535) {
       // Location too long, cut stack
-      auto newLen = 65536 - baseLength - detailsRef.size();
+      auto newLen = 65535 - baseLength - detailsRef.size();
       std::string newLocation{location, newLen};
       char empty = '\0';
       retval = FRC_NetworkCommunication_sendError(
           isError, errorCode, isLVCode, details, newLocation.c_str(), &empty);
     } else {
       // Stack too long
-      auto newLen = 65536 - baseLength - detailsRef.size() - locationRef.size();
+      auto newLen = 65535 - baseLength - detailsRef.size() - locationRef.size();
       std::string newCallStack{callStack, newLen};
       retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode,
                                                   details, location,
@@ -229,6 +229,18 @@
   return retval;
 }
 
+int32_t HAL_SendConsoleLine(const char* line) {
+  wpi::StringRef lineRef{line};
+  if (lineRef.size() <= 65535) {
+    // Send directly
+    return FRC_NetworkCommunication_sendConsoleLine(line);
+  } else {
+    // Need to truncate
+    std::string newLine{line, 65535};
+    return FRC_NetworkCommunication_sendConsoleLine(newLine.c_str());
+  }
+}
+
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   return HAL_GetControlWordInternal(controlWord);
 }
@@ -343,41 +355,14 @@
   // will return false when instead it should return true. However, this at a
   // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
   // worth the cycles to check.
-  thread_local int lastCount{-1};
+  thread_local int lastCount{0};
   return lastCount;
 }
 
-void HAL_WaitForCachedControlData(void) {
-  HAL_WaitForCachedControlDataTimeout(0);
-}
-
-HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
-  int& lastCount = GetThreadLocalLastCount();
-  int currentCount = newDSDataAvailableCounter.load();
-  if (lastCount != currentCount) {
-    lastCount = currentCount;
-    return true;
-  }
-  auto timeoutTime =
-      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
-
-  std::unique_lock lock{*newDSDataAvailableMutex};
-  while (newDSDataAvailableCounter.load() == currentCount) {
-    if (timeout > 0) {
-      auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
-      if (timedOut == std::cv_status::timeout) {
-        return false;
-      }
-    } else {
-      newDSDataAvailableCond->wait(lock);
-    }
-  }
-  return true;
-}
-
 HAL_Bool HAL_IsNewControlData(void) {
+  std::scoped_lock lock{*newDSDataAvailableMutex};
   int& lastCount = GetThreadLocalLastCount();
-  int currentCount = newDSDataAvailableCounter.load();
+  int currentCount = newDSDataAvailableCounter;
   if (lastCount == currentCount) return false;
   lastCount = currentCount;
   return true;
@@ -394,12 +379,17 @@
  * time has passed. Returns true on new data, false on timeout.
  */
 HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
+  std::unique_lock lock{*newDSDataAvailableMutex};
+  int& lastCount = GetThreadLocalLastCount();
+  int currentCount = newDSDataAvailableCounter;
+  if (lastCount != currentCount) {
+    lastCount = currentCount;
+    return true;
+  }
   auto timeoutTime =
       std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
 
-  int currentCount = newDSDataAvailableCounter.load();
-  std::unique_lock lock{*newDSDataAvailableMutex};
-  while (newDSDataAvailableCounter.load() == currentCount) {
+  while (newDSDataAvailableCounter == currentCount) {
     if (timeout > 0) {
       auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
       if (timedOut == std::cv_status::timeout) {
@@ -409,6 +399,7 @@
       newDSDataAvailableCond->wait(lock);
     }
   }
+  lastCount = newDSDataAvailableCounter;
   return true;
 }
 
@@ -419,8 +410,9 @@
   // Since we could get other values, require our specific handle
   // to signal our threads
   if (refNum != refNumber) return;
+  std::scoped_lock lock{*newDSDataAvailableMutex};
   // Notify all threads
-  newDSDataAvailableCounter.fetch_add(1);
+  ++newDSDataAvailableCounter;
   newDSDataAvailableCond->notify_all();
 }
 

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

diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp
index f698fa0..17ef0b9 100644
--- a/hal/src/main/native/athena/HAL.cpp
+++ b/hal/src/main/native/athena/HAL.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,12 +24,14 @@
 #include <wpi/timestamp.h>
 
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "ctre/ctre.h"
 #include "hal/ChipObject.h"
 #include "hal/DriverStation.h"
 #include "hal/Errors.h"
 #include "hal/Notifier.h"
 #include "hal/handles/HandlesInternal.h"
+#include "visa/visa.h"
 
 using namespace hal;
 
@@ -44,6 +46,7 @@
   InitializeAddressableLED();
   InitializeAccelerometer();
   InitializeAnalogAccumulator();
+  InitializeAnalogGyro();
   InitializeAnalogInput();
   InitializeAnalogInternal();
   InitializeAnalogOutput();
@@ -61,7 +64,7 @@
   InitializeFPGAEncoder();
   InitializeFRCDriverStation();
   InitializeI2C();
-  InitialzeInterrupts();
+  InitializeInterrupts();
   InitializeMain();
   InitializeNotifier();
   InitializePCMInternal();
@@ -70,11 +73,22 @@
   InitializePower();
   InitializePWM();
   InitializeRelay();
+  InitializeSerialPort();
   InitializeSolenoid();
   InitializeSPI();
   InitializeThreads();
 }
 }  // namespace init
+
+void ReleaseFPGAInterrupt(int32_t interruptNumber) {
+  if (!global) {
+    return;
+  }
+  int32_t status = 0;
+  global->writeInterruptForceNumber(static_cast<unsigned char>(interruptNumber),
+                                    &status);
+  global->strobeInterruptForceOnce(&status);
+}
 }  // namespace hal
 
 extern "C" {
@@ -170,6 +184,30 @@
       return ERR_CANSessionMux_NotAllowed_MESSAGE;
     case HAL_ERR_CANSessionMux_NotInitialized:
       return ERR_CANSessionMux_NotInitialized_MESSAGE;
+    case VI_ERROR_SYSTEM_ERROR:
+      return VI_ERROR_SYSTEM_ERROR_MESSAGE;
+    case VI_ERROR_INV_OBJECT:
+      return VI_ERROR_INV_OBJECT_MESSAGE;
+    case VI_ERROR_RSRC_LOCKED:
+      return VI_ERROR_RSRC_LOCKED_MESSAGE;
+    case VI_ERROR_RSRC_NFOUND:
+      return VI_ERROR_RSRC_NFOUND_MESSAGE;
+    case VI_ERROR_INV_RSRC_NAME:
+      return VI_ERROR_INV_RSRC_NAME_MESSAGE;
+    case VI_ERROR_QUEUE_OVERFLOW:
+      return VI_ERROR_QUEUE_OVERFLOW_MESSAGE;
+    case VI_ERROR_IO:
+      return VI_ERROR_IO_MESSAGE;
+    case VI_ERROR_ASRL_PARITY:
+      return VI_ERROR_ASRL_PARITY_MESSAGE;
+    case VI_ERROR_ASRL_FRAMING:
+      return VI_ERROR_ASRL_FRAMING_MESSAGE;
+    case VI_ERROR_ASRL_OVERRUN:
+      return VI_ERROR_ASRL_OVERRUN_MESSAGE;
+    case VI_ERROR_RSRC_BUSY:
+      return VI_ERROR_RSRC_BUSY_MESSAGE;
+    case VI_ERROR_INV_PARAMETER:
+      return VI_ERROR_INV_PARAMETER_MESSAGE;
     case HAL_PWM_SCALE_ERROR:
       return HAL_PWM_SCALE_ERROR_MESSAGE;
     case HAL_SERIAL_PORT_NOT_FOUND:
@@ -296,7 +334,7 @@
       kill(pid, SIGTERM);  // try to kill it
       std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
       if (kill(pid, 0) == 0) {
-        // still not successfull
+        // still not successful
         wpi::outs() << "FRC pid " << pid << " did not die within " << timeout
                     << "ms. Force killing with kill -9\n";
         // Force kill -9
@@ -352,9 +390,8 @@
     setNewDataSem(nullptr);
   });
 
-  // image 4; Fixes errors caused by multiple processes. Talk to NI about this
   nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass =
-      nLoadOut::kTargetClass_RoboRIO;
+      nLoadOut::getTargetClass();
 
   int32_t status = 0;
   global.reset(tGlobal::create(&status));
@@ -383,6 +420,8 @@
   return true;
 }
 
+void HAL_Shutdown(void) {}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   if (feature == nullptr) {

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

diff --git a/hal/src/main/native/athena/HALInitializer.cpp b/hal/src/main/native/athena/HALInitializer.cpp
index a0456d4..5c2242b 100644
--- a/hal/src/main/native/athena/HALInitializer.cpp
+++ b/hal/src/main/native/athena/HALInitializer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,7 @@
 
 #include "HALInitializer.h"
 
-#include "hal/HAL.h"
+#include "hal/HALBase.h"
 
 namespace hal {
 namespace init {

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

diff --git a/hal/src/main/native/athena/HALInitializer.h b/hal/src/main/native/athena/HALInitializer.h
index cf59394..c54660f 100644
--- a/hal/src/main/native/athena/HALInitializer.h
+++ b/hal/src/main/native/athena/HALInitializer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,7 @@
 extern void InitializeAccelerometer();
 extern void InitializeAddressableLED();
 extern void InitializeAnalogAccumulator();
+extern void InitializeAnalogGyro();
 extern void InitializeAnalogInput();
 extern void InitializeAnalogInternal();
 extern void InitializeAnalogOutput();
@@ -39,7 +40,7 @@
 extern void InitializeFRCDriverStation();
 extern void InitializeHAL();
 extern void InitializeI2C();
-extern void InitialzeInterrupts();
+extern void InitializeInterrupts();
 extern void InitializeMain();
 extern void InitializeNotifier();
 extern void InitializePCMInternal();
@@ -48,6 +49,7 @@
 extern void InitializePower();
 extern void InitializePWM();
 extern void InitializeRelay();
+extern void InitializeSerialPort();
 extern void InitializeSolenoid();
 extern void InitializeSPI();
 extern void InitializeThreads();

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

diff --git a/hal/src/main/native/athena/HALInternal.h b/hal/src/main/native/athena/HALInternal.h
new file mode 100644
index 0000000..e3033bf
--- /dev/null
+++ b/hal/src/main/native/athena/HALInternal.h
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+namespace hal {
+void ReleaseFPGAInterrupt(int32_t interruptNumber);
+
+}  // namespace hal

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

diff --git a/hal/src/main/native/athena/Interrupts.cpp b/hal/src/main/native/athena/Interrupts.cpp
index 78d518c..a330e3c 100644
--- a/hal/src/main/native/athena/Interrupts.cpp
+++ b/hal/src/main/native/athena/Interrupts.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,9 +13,11 @@
 
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/ChipObject.h"
 #include "hal/Errors.h"
+#include "hal/HALBase.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
 
@@ -82,7 +84,7 @@
 
 namespace hal {
 namespace init {
-void InitialzeInterrupts() {
+void InitializeInterrupts() {
   static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
                                HAL_HandleEnum::Interrupt>
       iH;
@@ -268,4 +270,19 @@
   anInterrupt->anInterrupt->writeConfig_FallingEdge(fallingEdge, status);
 }
 
+void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle,
+                                 int32_t* status) {
+  auto anInterrupt = interruptHandles->Get(interruptHandle);
+  if (anInterrupt == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint32_t interruptIndex =
+      static_cast<uint32_t>(getHandleIndex(interruptHandle));
+
+  hal::ReleaseFPGAInterrupt(interruptIndex);
+  hal::ReleaseFPGAInterrupt(interruptIndex + 8);
+}
+
 }  // extern "C"

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

diff --git a/hal/src/main/native/athena/Notifier.cpp b/hal/src/main/native/athena/Notifier.cpp
index c30e8d1..905c440 100644
--- a/hal/src/main/native/athena/Notifier.cpp
+++ b/hal/src/main/native/athena/Notifier.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,11 +10,13 @@
 #include <atomic>
 #include <cstdlib>  // For std::atexit()
 #include <memory>
+#include <thread>
 
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "hal/ChipObject.h"
 #include "hal/Errors.h"
 #include "hal/HAL.h"
@@ -26,7 +28,7 @@
 
 static wpi::mutex notifierMutex;
 static std::unique_ptr<tAlarm> notifierAlarm;
-static std::unique_ptr<tInterruptManager> notifierManager;
+static std::thread notifierThread;
 static uint64_t closestTrigger{UINT64_MAX};
 
 namespace {
@@ -43,6 +45,7 @@
 
 static std::atomic_flag notifierAtexitRegistered{ATOMIC_FLAG_INIT};
 static std::atomic_int notifierRefCount{0};
+static std::atomic_bool notifierRunning{false};
 
 using namespace hal;
 
@@ -65,7 +68,7 @@
 
 static NotifierHandleContainer* notifierHandles;
 
-static void alarmCallback(uint32_t, void*) {
+static void alarmCallback() {
   std::scoped_lock lock(notifierMutex);
   int32_t status = 0;
   uint64_t currentTime = 0;
@@ -97,9 +100,24 @@
   }
 }
 
+static void notifierThreadMain() {
+  tRioStatusCode status = 0;
+  tInterruptManager manager{1 << kTimerInterruptNumber, true, &status};
+  while (notifierRunning) {
+    auto triggeredMask = manager.watch(10000, false, &status);
+    if (!notifierRunning) break;
+    if (triggeredMask == 0) continue;
+    alarmCallback();
+  }
+}
+
 static void cleanupNotifierAtExit() {
+  int32_t status = 0;
+  if (notifierAlarm) notifierAlarm->writeEnable(false, &status);
   notifierAlarm = nullptr;
-  notifierManager = nullptr;
+  notifierRunning = false;
+  hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
+  if (notifierThread.joinable()) notifierThread.join();
 }
 
 namespace hal {
@@ -120,14 +138,9 @@
 
   if (notifierRefCount.fetch_add(1) == 0) {
     std::scoped_lock lock(notifierMutex);
-    // create manager and alarm if not already created
-    if (!notifierManager) {
-      notifierManager = std::make_unique<tInterruptManager>(
-          1 << kTimerInterruptNumber, false, status);
-      notifierManager->registerHandler(alarmCallback, nullptr, status);
-      notifierManager->enable(status);
-    }
-    if (!notifierAlarm) notifierAlarm.reset(tAlarm::create(status));
+    notifierRunning = true;
+    notifierThread = std::thread(notifierThreadMain);
+    notifierAlarm.reset(tAlarm::create(status));
   }
 
   std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
@@ -169,21 +182,19 @@
   notifier->cond.notify_all();
 
   if (notifierRefCount.fetch_sub(1) == 1) {
-    // if this was the last notifier, clean up alarm and manager
+    // if this was the last notifier, clean up alarm and thread
     // the notifier can call back into our callback, so don't hold the lock
     // here (the atomic fetch_sub will prevent multiple parallel entries
     // into this function)
 
-    // Cleaning up the manager takes up to a second to complete, so don't do
-    // that here. Fix it more permanently in 2019...
+    if (notifierAlarm) notifierAlarm->writeEnable(false, status);
+    notifierRunning = false;
+    hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
+    if (notifierThread.joinable()) notifierThread.join();
 
-    // if (notifierAlarm) notifierAlarm->writeEnable(false, status);
-    // if (notifierManager) notifierManager->disable(status);
-
-    // std::scoped_lock lock(notifierMutex);
-    // notifierAlarm = nullptr;
-    // notifierManager = nullptr;
-    // closestTrigger = UINT64_MAX;
+    std::scoped_lock lock(notifierMutex);
+    notifierAlarm = nullptr;
+    closestTrigger = UINT64_MAX;
   }
 }
 

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

diff --git a/hal/src/main/native/athena/PCMInternal.cpp b/hal/src/main/native/athena/PCMInternal.cpp
index dee64cf..c81dc1a 100644
--- a/hal/src/main/native/athena/PCMInternal.cpp
+++ b/hal/src/main/native/athena/PCMInternal.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,6 @@
 #include "PCMInternal.h"
 
 #include "HALInitializer.h"
-#include "PortsInternal.h"
 #include "hal/Errors.h"
 #include "hal/Solenoid.h"
 

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

diff --git a/hal/src/main/native/athena/PCMInternal.h b/hal/src/main/native/athena/PCMInternal.h
index 52f0f75..a9d076c 100644
--- a/hal/src/main/native/athena/PCMInternal.h
+++ b/hal/src/main/native/athena/PCMInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,7 +14,6 @@
 #include "PortsInternal.h"
 #include "ctre/PCM.h"
 #include "hal/Errors.h"
-#include "hal/Ports.h"
 #include "hal/Solenoid.h"
 
 namespace hal {

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

diff --git a/hal/src/main/native/athena/PDP.cpp b/hal/src/main/native/athena/PDP.cpp
index f5cf92b..f60e881 100644
--- a/hal/src/main/native/athena/PDP.cpp
+++ b/hal/src/main/native/athena/PDP.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,16 +7,12 @@
 
 #include "hal/PDP.h"
 
-#include <memory>
-
 #include <wpi/mutex.h>
 
 #include "HALInitializer.h"
 #include "PortsInternal.h"
 #include "hal/CANAPI.h"
 #include "hal/Errors.h"
-#include "hal/Ports.h"
-#include "hal/handles/IndexedHandleResource.h"
 
 using namespace hal;
 
@@ -438,7 +434,7 @@
   energyJoules *=
       pdpStatus.bits
           .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
-  return 0.125 * raw;
+  return energyJoules;
 }
 
 void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {

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

diff --git a/hal/src/main/native/athena/Relay.cpp b/hal/src/main/native/athena/Relay.cpp
index 71880a8..4dffa36 100644
--- a/hal/src/main/native/athena/Relay.cpp
+++ b/hal/src/main/native/athena/Relay.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -88,8 +88,8 @@
 
 HAL_Bool HAL_CheckRelayChannel(int32_t channel) {
   // roboRIO only has 4 headers, and the FPGA has
-  // seperate functions for forward and reverse,
-  // instead of seperate channel IDs
+  // separate functions for forward and reverse,
+  // instead of separate channel IDs
   return channel < kNumRelayHeaders && channel >= 0;
 }
 

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

diff --git a/hal/src/main/native/athena/SPI.cpp b/hal/src/main/native/athena/SPI.cpp
index 977d447..37c5f0e 100644
--- a/hal/src/main/native/athena/SPI.cpp
+++ b/hal/src/main/native/athena/SPI.cpp
@@ -565,18 +565,12 @@
 void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
                                 int32_t dataSize, int32_t zeroSize,
                                 int32_t* status) {
-  static_assert(tSPI::kNumAutoTxRegisters >= 6,
-                "FPGA does not have enough tx registers");
-  static_assert(tSPI::kNumAutoTxElements == 4,
-                "FPGA has the wrong number of tx elements");
-  // 24 = 6 * 4, but the documentation needs updating if it ever changes, so
-  // just hard-code it here.
-  if (dataSize < 0 || dataSize > 23) {
+  if (dataSize < 0 || dataSize > 32) {
     *status = PARAMETER_OUT_OF_RANGE;
     return;
   }
 
-  if (zeroSize < 0 || zeroSize >= 128) {
+  if (zeroSize < 0 || zeroSize > 127) {
     *status = PARAMETER_OUT_OF_RANGE;
     return;
   }

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

diff --git a/hal/src/main/native/athena/Solenoid.cpp b/hal/src/main/native/athena/Solenoid.cpp
index c53db11..ab563b3 100644
--- a/hal/src/main/native/athena/Solenoid.cpp
+++ b/hal/src/main/native/athena/Solenoid.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,15 +7,11 @@
 
 #include "hal/Solenoid.h"
 
-#include <FRC_NetworkCommunication/LoadOut.h>
-
 #include "HALInitializer.h"
 #include "PCMInternal.h"
 #include "PortsInternal.h"
 #include "ctre/PCM.h"
-#include "hal/ChipObject.h"
 #include "hal/Errors.h"
-#include "hal/Ports.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
 

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

diff --git a/hal/src/main/native/athena/cpp/SerialHelper.cpp b/hal/src/main/native/athena/cpp/SerialHelper.cpp
index e312e2a..2156147 100644
--- a/hal/src/main/native/athena/cpp/SerialHelper.cpp
+++ b/hal/src/main/native/athena/cpp/SerialHelper.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -199,7 +199,7 @@
 
     *status = viGetAttribute(vSession, VI_ATTR_INTF_INST_NAME, &osName);
     // Ignore an error here, as we want to close the session on an error
-    // Use a seperate close variable so we can check
+    // Use a separate close variable so we can check
     ViStatus closeStatus = viClose(vSession);
     if (*status < 0) goto done;
     if (closeStatus < 0) goto done;

hal/src/main/native/athena/ctre/CtreCanNode.h

diff --git a/hal/src/main/native/athena/ctre/CtreCanNode.h b/hal/src/main/native/athena/ctre/CtreCanNode.h
index f00060d..ce2d6ac 100644
--- a/hal/src/main/native/athena/ctre/CtreCanNode.h
+++ b/hal/src/main/native/athena/ctre/CtreCanNode.h
@@ -2,8 +2,6 @@
 #define CtreCanNode_H_
 #include "ctre.h"				//BIT Defines + Typedefs
 #include <map>
-#include <string.h> // memcpy
-#include <sys/time.h>
 #include <wpi/mutex.h>
 class CtreCanNode
 {

hal/src/main/native/athena/ctre/PCM.cpp

diff --git a/hal/src/main/native/athena/ctre/PCM.cpp b/hal/src/main/native/athena/ctre/PCM.cpp
index e05d4d4..4fd633f 100644
--- a/hal/src/main/native/athena/ctre/PCM.cpp
+++ b/hal/src/main/native/athena/ctre/PCM.cpp
@@ -2,7 +2,6 @@
 
 #include "PCM.h"
 #include "FRC_NetworkCommunication/CANSessionMux.h"
-#include <string.h> // memset
 /* This can be a constant, as long as nobody needs to update solenoids within
     1/50 of a second. */
 static const INT32 kCANPeriod = 20;

hal/src/main/native/athena/ctre/PCM.h

diff --git a/hal/src/main/native/athena/ctre/PCM.h b/hal/src/main/native/athena/ctre/PCM.h
index b485219..4923202 100644
--- a/hal/src/main/native/athena/ctre/PCM.h
+++ b/hal/src/main/native/athena/ctre/PCM.h
@@ -57,7 +57,7 @@
 
     /* Get compressor state
      * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if compress ouput is on, false if otherwise
+     * @Param	-	status		-	True if compress output is on, false if otherwise
      */
     CTR_Code	GetCompressor(bool &status);
 

hal/src/main/native/athena/mockdata/AccelerometerData.cpp

diff --git a/hal/src/main/native/athena/mockdata/AccelerometerData.cpp b/hal/src/main/native/athena/mockdata/AccelerometerData.cpp
new file mode 100644
index 0000000..2baaf78
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/AccelerometerData.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/AccelerometerData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetAccelerometerData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Accelerometer##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Active, false)
+DEFINE_CAPI(HAL_AccelerometerRange, Range, HAL_AccelerometerRange_k2G)
+DEFINE_CAPI(double, X, 0)
+DEFINE_CAPI(double, Y, 0)
+DEFINE_CAPI(double, Z, 0)
+
+void HALSIM_RegisterAccelerometerAllCallbacks(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/AddressableLEDData.cpp

diff --git a/hal/src/main/native/athena/mockdata/AddressableLEDData.cpp b/hal/src/main/native/athena/mockdata/AddressableLEDData.cpp
new file mode 100644
index 0000000..d718789
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/AddressableLEDData.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/AddressableLEDData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+
+int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) { return 0; }
+
+void HALSIM_ResetAddressableLEDData(int32_t index) {}
+
+int32_t HALSIM_GetAddressableLEDData(int32_t index,
+                                     struct HAL_AddressableLEDData* data) {
+  return 0;
+}
+
+void HALSIM_SetAddressableLEDData(int32_t index,
+                                  const struct HAL_AddressableLEDData* data,
+                                  int32_t length) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(int32_t, OutputPort, 0)
+DEFINE_CAPI(int32_t, Length, 0)
+DEFINE_CAPI(HAL_Bool, Running, false)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, AddressableLED##CAPINAME)
+
+DEFINE_CAPI(HAL_ConstBufferCallback, Data, data)
+
+void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/AnalogGyroData.cpp

diff --git a/hal/src/main/native/athena/mockdata/AnalogGyroData.cpp b/hal/src/main/native/athena/mockdata/AnalogGyroData.cpp
new file mode 100644
index 0000000..d91ce27
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/AnalogGyroData.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/AnalogGyroData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetAnalogGyroData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogGyro##CAPINAME, RETURN)
+
+DEFINE_CAPI(double, Angle, 0)
+DEFINE_CAPI(double, Rate, 0)
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+
+void HALSIM_RegisterAnalogGyroAllCallbacks(int32_t index,
+                                           HAL_NotifyCallback callback,
+                                           void* param,
+                                           HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/AnalogInData.cpp

diff --git a/hal/src/main/native/athena/mockdata/AnalogInData.cpp b/hal/src/main/native/athena/mockdata/AnalogInData.cpp
new file mode 100644
index 0000000..4288538
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/AnalogInData.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/AnalogInData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetAnalogInData(int32_t index) {}
+
+HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index) { return 0; }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogIn##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(int32_t, AverageBits, 0)
+DEFINE_CAPI(int32_t, OversampleBits, 0)
+DEFINE_CAPI(double, Voltage, 0)
+DEFINE_CAPI(HAL_Bool, AccumulatorInitialized, false)
+DEFINE_CAPI(int64_t, AccumulatorValue, 0)
+DEFINE_CAPI(int64_t, AccumulatorCount, 0)
+DEFINE_CAPI(int32_t, AccumulatorCenter, 0)
+DEFINE_CAPI(int32_t, AccumulatorDeadband, 0)
+
+void HALSIM_RegisterAnalogInAllCallbacks(int32_t index,
+                                         HAL_NotifyCallback callback,
+                                         void* param, HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/AnalogOutData.cpp

diff --git a/hal/src/main/native/athena/mockdata/AnalogOutData.cpp b/hal/src/main/native/athena/mockdata/AnalogOutData.cpp
new file mode 100644
index 0000000..f05b0be
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/AnalogOutData.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/AnalogOutData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetAnalogOutData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogOut##CAPINAME, RETURN)
+
+DEFINE_CAPI(double, Voltage, 0)
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+
+void HALSIM_RegisterAnalogOutAllCallbacks(int32_t index,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify) {
+}
+}  // extern "C"

hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp

diff --git a/hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp b/hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp
new file mode 100644
index 0000000..7781cf2
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/AnalogTriggerData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+
+int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel) { return 0; }
+
+void HALSIM_ResetAnalogTriggerData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogTrigger##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(double, TriggerLowerBound, 0)
+DEFINE_CAPI(double, TriggerUpperBound, 0)
+DEFINE_CAPI(HALSIM_AnalogTriggerMode, TriggerMode,
+            HALSIM_AnalogTriggerUnassigned)
+
+void HALSIM_RegisterAnalogTriggerAllCallbacks(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/CanDataInternal.cpp

diff --git a/hal/src/main/native/athena/mockdata/CanDataInternal.cpp b/hal/src/main/native/athena/mockdata/CanDataInternal.cpp
new file mode 100644
index 0000000..69debf0
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/CanDataInternal.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/CanData.h"
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+
+void HALSIM_ResetCanData(void) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME) \
+  HAL_SIMCALLBACKREGISTRY_STUB_CAPI_NOINDEX(TYPE, HALSIM, Can##CAPINAME)
+
+DEFINE_CAPI(HAL_CAN_SendMessageCallback, SendMessage)
+DEFINE_CAPI(HAL_CAN_ReceiveMessageCallback, ReceiveMessage)
+DEFINE_CAPI(HAL_CAN_OpenStreamSessionCallback, OpenStream)
+DEFINE_CAPI(HAL_CAN_CloseStreamSessionCallback, CloseStream)
+DEFINE_CAPI(HAL_CAN_ReadStreamSessionCallback, ReadStream)
+DEFINE_CAPI(HAL_CAN_GetCANStatusCallback, GetCANStatus)
+
+}  // extern "C"

hal/src/main/native/athena/mockdata/DIOData.cpp

diff --git a/hal/src/main/native/athena/mockdata/DIOData.cpp b/hal/src/main/native/athena/mockdata/DIOData.cpp
new file mode 100644
index 0000000..a06855d
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/DIOData.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/DIOData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetDIOData(int32_t index) {}
+
+HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index) { return 0; }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DIO##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(HAL_Bool, Value, false)
+DEFINE_CAPI(double, PulseLength, 0)
+DEFINE_CAPI(HAL_Bool, IsInput, false)
+DEFINE_CAPI(int32_t, FilterIndex, 0)
+
+void HALSIM_RegisterDIOAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+                                    void* param, HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/DigitalPWMData.cpp

diff --git a/hal/src/main/native/athena/mockdata/DigitalPWMData.cpp b/hal/src/main/native/athena/mockdata/DigitalPWMData.cpp
new file mode 100644
index 0000000..bcbe370
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/DigitalPWMData.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/DigitalPWMData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel) { return 0; }
+
+void HALSIM_ResetDigitalPWMData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DigitalPWM##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(double, DutyCycle, 0)
+DEFINE_CAPI(int32_t, Pin, 0)
+
+void HALSIM_RegisterDigitalPWMAllCallbacks(int32_t index,
+                                           HAL_NotifyCallback callback,
+                                           void* param,
+                                           HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/DriverStationData.cpp

diff --git a/hal/src/main/native/athena/mockdata/DriverStationData.cpp b/hal/src/main/native/athena/mockdata/DriverStationData.cpp
new file mode 100644
index 0000000..b4695ba
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/DriverStationData.cpp
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/DriverStationData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetDriverStationData(void) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN)                                 \
+  HAL_SIMDATAVALUE_STUB_CAPI_NOINDEX(TYPE, HALSIM, DriverStation##CAPINAME, \
+                                     RETURN)
+
+DEFINE_CAPI(HAL_Bool, Enabled, false)
+DEFINE_CAPI(HAL_Bool, Autonomous, false)
+DEFINE_CAPI(HAL_Bool, Test, false)
+DEFINE_CAPI(HAL_Bool, EStop, false)
+DEFINE_CAPI(HAL_Bool, FmsAttached, false)
+DEFINE_CAPI(HAL_Bool, DsAttached, false)
+DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId,
+            HAL_AllianceStationID_kRed1)
+DEFINE_CAPI(double, MatchTime, 0)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(name, data)                                                \
+  int32_t HALSIM_RegisterJoystick##name##Callback(                             \
+      int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
+      HAL_Bool initialNotify) {                                                \
+    return 0;                                                                  \
+  }                                                                            \
+                                                                               \
+  void HALSIM_CancelJoystick##name##Callback(int32_t uid) {}                   \
+                                                                               \
+  void HALSIM_GetJoystick##name(int32_t joystickNum, HAL_Joystick##name* d) {} \
+                                                                               \
+  void HALSIM_SetJoystick##name(int32_t joystickNum,                           \
+                                const HAL_Joystick##name* d) {}
+
+DEFINE_CAPI(Axes, axes)
+DEFINE_CAPI(POVs, povs)
+DEFINE_CAPI(Buttons, buttons)
+DEFINE_CAPI(Descriptor, descriptor)
+
+int32_t HALSIM_RegisterJoystickOutputsCallback(
+    int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
+    HAL_Bool initialNotify) {
+  return 0;
+}
+
+void HALSIM_CancelJoystickOutputsCallback(int32_t uid) {}
+
+void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
+                               int32_t* leftRumble, int32_t* rightRumble) {}
+
+void HALSIM_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+                               int32_t leftRumble, int32_t rightRumble) {}
+
+int32_t HALSIM_RegisterMatchInfoCallback(HAL_MatchInfoCallback callback,
+                                         void* param, HAL_Bool initialNotify) {
+  return 0;
+}
+
+void HALSIM_CancelMatchInfoCallback(int32_t uid) {}
+
+void HALSIM_GetMatchInfo(HAL_MatchInfo* info) {}
+
+void HALSIM_SetMatchInfo(const HAL_MatchInfo* info) {}
+
+int32_t HALSIM_RegisterDriverStationNewDataCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify) {
+  return 0;
+}
+
+void HALSIM_CancelDriverStationNewDataCallback(int32_t uid) {}
+
+void HALSIM_NotifyDriverStationNewData(void) {}
+
+void HALSIM_SetJoystickButton(int32_t stick, int32_t button, HAL_Bool state) {}
+
+void HALSIM_SetJoystickAxis(int32_t stick, int32_t axis, double value) {}
+
+void HALSIM_SetJoystickPOV(int32_t stick, int32_t pov, int32_t value) {}
+
+void HALSIM_SetJoystickButtonsValue(int32_t stick, uint32_t buttons) {}
+
+void HALSIM_SetJoystickAxisCount(int32_t stick, int32_t count) {}
+
+void HALSIM_SetJoystickPOVCount(int32_t stick, int32_t count) {}
+
+void HALSIM_SetJoystickButtonCount(int32_t stick, int32_t count) {}
+
+void HALSIM_GetJoystickCounts(int32_t stick, int32_t* axisCount,
+                              int32_t* buttonCount, int32_t* povCount) {
+  *axisCount = 0;
+  *buttonCount = 0;
+  *povCount = 0;
+}
+
+void HALSIM_SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {}
+
+void HALSIM_SetJoystickType(int32_t stick, int32_t type) {}
+
+void HALSIM_SetJoystickName(int32_t stick, const char* name) {}
+
+void HALSIM_SetJoystickAxisType(int32_t stick, int32_t axis, int32_t type) {}
+
+void HALSIM_SetGameSpecificMessage(const char* message) {}
+
+void HALSIM_SetEventName(const char* name) {}
+
+void HALSIM_SetMatchType(HAL_MatchType type) {}
+
+void HALSIM_SetMatchNumber(int32_t matchNumber) {}
+
+void HALSIM_SetReplayNumber(int32_t replayNumber) {}
+
+void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/DutyCycleData.cpp

diff --git a/hal/src/main/native/athena/mockdata/DutyCycleData.cpp b/hal/src/main/native/athena/mockdata/DutyCycleData.cpp
new file mode 100644
index 0000000..8d3cd61
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/DutyCycleData.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/DutyCycleData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) { return 0; }
+
+void HALSIM_ResetDutyCycleData(int32_t index) {}
+
+int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) { return 0; }
+
+HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) { return 0; }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DutyCycle##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(int32_t, Frequency, 0)
+DEFINE_CAPI(double, Output, 0)
+
+void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify) {
+}
+
+}  // extern "C"

hal/src/main/native/athena/mockdata/EncoderData.cpp

diff --git a/hal/src/main/native/athena/mockdata/EncoderData.cpp b/hal/src/main/native/athena/mockdata/EncoderData.cpp
new file mode 100644
index 0000000..87a7385
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/EncoderData.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/EncoderData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+int32_t HALSIM_FindEncoderForChannel(int32_t channel) { return 0; }
+
+void HALSIM_ResetEncoderData(int32_t index) {}
+
+int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index) { return 0; }
+
+int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index) { return 0; }
+
+HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index) { return 0; }
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Encoder##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(int32_t, Count, 0)
+DEFINE_CAPI(double, Period, 0)
+DEFINE_CAPI(HAL_Bool, Reset, false)
+DEFINE_CAPI(double, MaxPeriod, 0)
+DEFINE_CAPI(HAL_Bool, Direction, false)
+DEFINE_CAPI(HAL_Bool, ReverseDirection, false)
+DEFINE_CAPI(int32_t, SamplesToAverage, 0)
+DEFINE_CAPI(double, DistancePerPulse, 0)
+
+void HALSIM_SetEncoderDistance(int32_t index, double distance) {}
+
+double HALSIM_GetEncoderDistance(int32_t index) { return 0; }
+
+void HALSIM_SetEncoderRate(int32_t index, double rate) {}
+
+double HALSIM_GetEncoderRate(int32_t index) { return 0; }
+
+void HALSIM_RegisterEncoderAllCallbacks(int32_t index,
+                                        HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/I2CData.cpp

diff --git a/hal/src/main/native/athena/mockdata/I2CData.cpp b/hal/src/main/native/athena/mockdata/I2CData.cpp
new file mode 100644
index 0000000..eb6a6c9
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/I2CData.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/I2CData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetI2CData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, I2C##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(TYPE, CAPINAME) \
+  HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, I2C##CAPINAME)
+
+DEFINE_CAPI(HAL_BufferCallback, Read)
+DEFINE_CAPI(HAL_ConstBufferCallback, Write)
+
+}  // extern "C"

hal/src/main/native/athena/mockdata/MockHooks.cpp

diff --git a/hal/src/main/native/athena/mockdata/MockHooks.cpp b/hal/src/main/native/athena/mockdata/MockHooks.cpp
new file mode 100644
index 0000000..2e7bcfe
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/MockHooks.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/MockHooks.h"
+
+extern "C" {
+
+void HALSIM_SetRuntimeType(HAL_RuntimeType type) {}
+
+void HALSIM_WaitForProgramStart(void) {}
+
+void HALSIM_SetProgramStarted(void) {}
+
+HAL_Bool HALSIM_GetProgramStarted(void) { return false; }
+
+void HALSIM_RestartTiming(void) {}
+
+void HALSIM_PauseTiming(void) {}
+
+void HALSIM_ResumeTiming(void) {}
+
+HAL_Bool HALSIM_IsTimingPaused(void) { return false; }
+
+void HALSIM_StepTiming(uint64_t delta) {}
+
+void HALSIM_StepTimingAsync(uint64_t delta) {}
+
+void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {}
+
+void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {}
+
+}  // extern "C"

hal/src/main/native/athena/mockdata/NotifierData.cpp

diff --git a/hal/src/main/native/athena/mockdata/NotifierData.cpp b/hal/src/main/native/athena/mockdata/NotifierData.cpp
new file mode 100644
index 0000000..34aa6e7
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/NotifierData.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/NotifierData.h"
+
+extern "C" {
+
+uint64_t HALSIM_GetNextNotifierTimeout(void) { return 0; }
+
+int32_t HALSIM_GetNumNotifiers(void) { return 0; }
+
+int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
+  return 0;
+}
+
+}  // extern "C"

hal/src/main/native/athena/mockdata/PCMData.cpp

diff --git a/hal/src/main/native/athena/mockdata/PCMData.cpp b/hal/src/main/native/athena/mockdata/PCMData.cpp
new file mode 100644
index 0000000..29302cb
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/PCMData.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/PCMData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetPCMData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PCM##CAPINAME, RETURN)
+
+HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidInitialized,
+                                   false)
+HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidOutput, false)
+DEFINE_CAPI(HAL_Bool, CompressorInitialized, false)
+DEFINE_CAPI(HAL_Bool, CompressorOn, false)
+DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, false)
+DEFINE_CAPI(HAL_Bool, PressureSwitch, false)
+DEFINE_CAPI(double, CompressorCurrent, 0)
+
+void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values) { *values = 0; }
+
+void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values) {}
+
+void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify) {}
+
+void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/PDPData.cpp

diff --git a/hal/src/main/native/athena/mockdata/PDPData.cpp b/hal/src/main/native/athena/mockdata/PDPData.cpp
new file mode 100644
index 0000000..a28bb81
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/PDPData.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/PDPData.h"
+
+#include "../PortsInternal.h"
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetPDPData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PDP##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(double, Temperature, 0)
+DEFINE_CAPI(double, Voltage, 0)
+HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(double, HALSIM, PDPCurrent, 0)
+
+void HALSIM_GetPDPAllCurrents(int32_t index, double* currents) {
+  for (int i = 0; i < hal::kNumPDPChannels; i++) currents[i] = 0;
+}
+
+void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents) {}
+
+void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/PWMData.cpp

diff --git a/hal/src/main/native/athena/mockdata/PWMData.cpp b/hal/src/main/native/athena/mockdata/PWMData.cpp
new file mode 100644
index 0000000..b9c5691
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/PWMData.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/PWMData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetPWMData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PWM##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(int32_t, RawValue, 0)
+DEFINE_CAPI(double, Speed, 0)
+DEFINE_CAPI(double, Position, 0)
+DEFINE_CAPI(int32_t, PeriodScale, 0)
+DEFINE_CAPI(HAL_Bool, ZeroLatch, false)
+
+void HALSIM_RegisterPWMAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+                                    void* param, HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/RelayData.cpp

diff --git a/hal/src/main/native/athena/mockdata/RelayData.cpp b/hal/src/main/native/athena/mockdata/RelayData.cpp
new file mode 100644
index 0000000..734bf89
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/RelayData.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/RelayData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetRelayData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Relay##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, InitializedForward, false)
+DEFINE_CAPI(HAL_Bool, InitializedReverse, false)
+DEFINE_CAPI(HAL_Bool, Forward, false)
+DEFINE_CAPI(HAL_Bool, Reverse, false)
+
+void HALSIM_RegisterRelayAllCallbacks(int32_t index,
+                                      HAL_NotifyCallback callback, void* param,
+                                      HAL_Bool initialNotify) {}
+}  // extern "C"

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

diff --git a/hal/src/main/native/athena/mockdata/RoboRioData.cpp b/hal/src/main/native/athena/mockdata/RoboRioData.cpp
new file mode 100644
index 0000000..f9e9ca7
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/RoboRioData.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/RoboRioData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetRoboRioData(void) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI_NOINDEX(TYPE, HALSIM, RoboRio##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, FPGAButton, false)
+DEFINE_CAPI(double, VInVoltage, 0)
+DEFINE_CAPI(double, VInCurrent, 0)
+DEFINE_CAPI(double, UserVoltage6V, 0)
+DEFINE_CAPI(double, UserCurrent6V, 0)
+DEFINE_CAPI(HAL_Bool, UserActive6V, false)
+DEFINE_CAPI(double, UserVoltage5V, 0)
+DEFINE_CAPI(double, UserCurrent5V, 0)
+DEFINE_CAPI(HAL_Bool, UserActive5V, false)
+DEFINE_CAPI(double, UserVoltage3V3, 0)
+DEFINE_CAPI(double, UserCurrent3V3, 0)
+DEFINE_CAPI(HAL_Bool, UserActive3V3, false)
+DEFINE_CAPI(int32_t, UserFaults6V, 0)
+DEFINE_CAPI(int32_t, UserFaults5V, 0)
+DEFINE_CAPI(int32_t, UserFaults3V3, 0)
+
+void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp

diff --git a/hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp b/hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp
new file mode 100644
index 0000000..c3e8ede
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/SPIAccelerometerData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetSPIAccelerometerData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPIAccelerometer##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Active, false)
+DEFINE_CAPI(int32_t, Range, 0)
+DEFINE_CAPI(double, X, 0)
+DEFINE_CAPI(double, Y, 0)
+DEFINE_CAPI(double, Z, 0)
+
+void HALSIM_RegisterSPIAccelerometerAllCallbcaks(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify) {}
+}  // extern "C"

hal/src/main/native/athena/mockdata/SPIData.cpp

diff --git a/hal/src/main/native/athena/mockdata/SPIData.cpp b/hal/src/main/native/athena/mockdata/SPIData.cpp
new file mode 100644
index 0000000..13cf67c
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/SPIData.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/SPIData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetSPIData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(TYPE, CAPINAME) \
+  HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME)
+
+DEFINE_CAPI(HAL_BufferCallback, Read)
+DEFINE_CAPI(HAL_ConstBufferCallback, Write)
+DEFINE_CAPI(HAL_SpiReadAutoReceiveBufferCallback, ReadAutoReceivedData)
+
+}  // extern "C"

hal/src/main/native/athena/mockdata/SimDeviceData.cpp

diff --git a/hal/src/main/native/athena/mockdata/SimDeviceData.cpp b/hal/src/main/native/athena/mockdata/SimDeviceData.cpp
new file mode 100644
index 0000000..a31160c
--- /dev/null
+++ b/hal/src/main/native/athena/mockdata/SimDeviceData.cpp
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "hal/simulation/SimDeviceData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+
+void HALSIM_SetSimDeviceEnabled(const char* prefix, HAL_Bool enabled) {}
+
+HAL_Bool HALSIM_IsSimDeviceEnabled(const char* name) { return false; }
+
+int32_t HALSIM_RegisterSimDeviceCreatedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
+    HAL_Bool initialNotify) {
+  return 0;
+}
+
+void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {}
+
+int32_t HALSIM_RegisterSimDeviceFreedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+  return 0;
+}
+
+void HALSIM_CancelSimDeviceFreedCallback(int32_t uid) {}
+
+HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) { return 0; }
+
+const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) { return ""; }
+
+HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) {
+  return 0;
+}
+
+void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
+                                HALSIM_SimDeviceCallback callback) {}
+
+int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
+                                               void* param,
+                                               HALSIM_SimValueCallback callback,
+                                               HAL_Bool initialNotify) {
+  return 0;
+}
+
+void HALSIM_CancelSimValueCreatedCallback(int32_t uid) {}
+
+int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
+                                               void* param,
+                                               HALSIM_SimValueCallback callback,
+                                               HAL_Bool initialNotify) {
+  return 0;
+}
+
+void HALSIM_CancelSimValueChangedCallback(int32_t uid) {}
+
+HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
+                                            const char* name) {
+  return 0;
+}
+
+void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
+                               HALSIM_SimValueCallback callback) {}
+
+const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
+                                           int32_t* numOptions) {
+  *numOptions = 0;
+  return nullptr;
+}
+
+void HALSIM_ResetSimDeviceData(void) {}
+
+}  // extern "C"

hal/src/main/native/cpp/cpp/fpga_clock.cpp

diff --git a/hal/src/main/native/cpp/cpp/fpga_clock.cpp b/hal/src/main/native/cpp/cpp/fpga_clock.cpp
index bcec155..ae65ee7 100644
--- a/hal/src/main/native/cpp/cpp/fpga_clock.cpp
+++ b/hal/src/main/native/cpp/cpp/fpga_clock.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,9 +7,11 @@
 
 #include "hal/cpp/fpga_clock.h"
 
+#include <limits>
+
 #include <wpi/raw_ostream.h>
 
-#include "hal/HAL.h"
+#include "hal/HALBase.h"
 
 namespace hal {
 const fpga_clock::time_point fpga_clock::min_time =

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

diff --git a/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
index 3f76e6a..3d74b24 100644
--- a/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
+++ b/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 #include "edu_wpi_first_hal_AddressableLEDJNI.h"
 #include "hal/AddressableLED.h"
 
-using namespace frc;
+using namespace hal;
 using namespace wpi::java;
 
 static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));

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

diff --git a/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp b/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
index 35b7414..c06e513 100644
--- a/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
+++ b/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,7 +14,7 @@
 #include "hal/AnalogGyro.h"
 #include "hal/handles/HandlesInternal.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 /*

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

diff --git a/hal/src/main/native/cpp/jni/AnalogJNI.cpp b/hal/src/main/native/cpp/jni/AnalogJNI.cpp
index 5571d55..8b920ac 100644
--- a/hal/src/main/native/cpp/jni/AnalogJNI.cpp
+++ b/hal/src/main/native/cpp/jni/AnalogJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,7 +18,7 @@
 #include "hal/Ports.h"
 #include "hal/handles/HandlesInternal.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/CANAPIJNI.cpp b/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
index 7ac7cf6..f42cc2f 100644
--- a/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
+++ b/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,7 +19,7 @@
 #include "hal/CANAPI.h"
 #include "hal/Errors.h"
 
-using namespace frc;
+using namespace hal;
 using namespace wpi::java;
 
 extern "C" {
@@ -109,6 +109,59 @@
 
 /*
  * Class:     edu_wpi_first_hal_CANAPIJNI
+ * Method:    writeCANPacketNoThrow
+ * Signature: (I[BI)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_writeCANPacketNoThrow
+  (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  JByteArrayRef arr{env, data};
+  auto arrRef = arr.array();
+  int32_t status = 0;
+  HAL_WriteCANPacket(halHandle, reinterpret_cast<const uint8_t*>(arrRef.data()),
+                     arrRef.size(), apiId, &status);
+  return status;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CANAPIJNI
+ * Method:    writeCANPacketRepeatingNoThrow
+ * Signature: (I[BII)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_writeCANPacketRepeatingNoThrow
+  (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId,
+   jint timeoutMs)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  JByteArrayRef arr{env, data};
+  auto arrRef = arr.array();
+  int32_t status = 0;
+  HAL_WriteCANPacketRepeating(halHandle,
+                              reinterpret_cast<const uint8_t*>(arrRef.data()),
+                              arrRef.size(), apiId, timeoutMs, &status);
+  return status;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CANAPIJNI
+ * Method:    writeCANRTRFrameNoThrow
+ * Signature: (III)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CANAPIJNI_writeCANRTRFrameNoThrow
+  (JNIEnv* env, jclass, jint handle, jint length, jint apiId)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  int32_t status = 0;
+  HAL_WriteCANRTRFrame(halHandle, static_cast<int32_t>(length), apiId, &status);
+  return status;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CANAPIJNI
  * Method:    stopCANPacketRepeating
  * Signature: (II)V
  */

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

diff --git a/hal/src/main/native/cpp/jni/CANJNI.cpp b/hal/src/main/native/cpp/jni/CANJNI.cpp
index 9278c24..42b484b 100644
--- a/hal/src/main/native/cpp/jni/CANJNI.cpp
+++ b/hal/src/main/native/cpp/jni/CANJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,7 +17,7 @@
 #include "edu_wpi_first_hal_can_CANJNI.h"
 #include "hal/CAN.h"
 
-using namespace frc;
+using namespace hal;
 using namespace wpi::java;
 
 extern "C" {

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

diff --git a/hal/src/main/native/cpp/jni/CompressorJNI.cpp b/hal/src/main/native/cpp/jni/CompressorJNI.cpp
index 75a5e0a..c214c60 100644
--- a/hal/src/main/native/cpp/jni/CompressorJNI.cpp
+++ b/hal/src/main/native/cpp/jni/CompressorJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 #include "hal/Ports.h"
 #include "hal/Solenoid.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/ConstantsJNI.cpp b/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
index fb1ae0b..9c4aa63 100644
--- a/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
+++ b/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 #include "edu_wpi_first_hal_ConstantsJNI.h"
 #include "hal/Constants.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 /*

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

diff --git a/hal/src/main/native/cpp/jni/CounterJNI.cpp b/hal/src/main/native/cpp/jni/CounterJNI.cpp
index 41dedab..06cbceb 100644
--- a/hal/src/main/native/cpp/jni/CounterJNI.cpp
+++ b/hal/src/main/native/cpp/jni/CounterJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,7 +14,7 @@
 #include "hal/Counter.h"
 #include "hal/Errors.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/DIOJNI.cpp b/hal/src/main/native/cpp/jni/DIOJNI.cpp
index 9c44b4c..428dd99 100644
--- a/hal/src/main/native/cpp/jni/DIOJNI.cpp
+++ b/hal/src/main/native/cpp/jni/DIOJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,7 +16,7 @@
 #include "hal/Ports.h"
 #include "hal/handles/HandlesInternal.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp b/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
index fbbaadb..cfb504d 100644
--- a/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
+++ b/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 #include "edu_wpi_first_hal_DigitalGlitchFilterJNI.h"
 #include "hal/DIO.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp b/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
index 510ca00..b191374 100644
--- a/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
+++ b/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 #include "edu_wpi_first_hal_DutyCycleJNI.h"
 #include "hal/DutyCycle.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 /*

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

diff --git a/hal/src/main/native/cpp/jni/EncoderJNI.cpp b/hal/src/main/native/cpp/jni/EncoderJNI.cpp
index e5aa7e8..11686a6 100644
--- a/hal/src/main/native/cpp/jni/EncoderJNI.cpp
+++ b/hal/src/main/native/cpp/jni/EncoderJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,7 +14,7 @@
 #include "hal/Encoder.h"
 #include "hal/Errors.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp
index 393b0b4..3d736bf 100644
--- a/hal/src/main/native/cpp/jni/HAL.cpp
+++ b/hal/src/main/native/cpp/jni/HAL.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,7 +19,7 @@
 #include "hal/DriverStation.h"
 #include "hal/Main.h"
 
-using namespace frc;
+using namespace hal;
 using namespace wpi::java;
 
 extern "C" {
@@ -38,6 +38,18 @@
 
 /*
  * Class:     edu_wpi_first_hal_HAL
+ * Method:    shutdown
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_shutdown
+  (JNIEnv*, jclass)
+{
+  return HAL_Shutdown();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_HAL
  * Method:    hasMain
  * Signature: ()Z
  */
@@ -450,6 +462,21 @@
 
 /*
  * Class:     edu_wpi_first_hal_HAL
+ * Method:    sendConsoleLine
+ * Signature: (Ljava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_HAL_sendConsoleLine
+  (JNIEnv* env, jclass, jstring line)
+{
+  JStringRef lineStr{env, line};
+
+  jint returnValue = HAL_SendConsoleLine(lineStr.c_str());
+  return returnValue;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_HAL
  * Method:    getPortWithModule
  * Signature: (BB)I
  */

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

diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp
index 26b7919..2938f45 100644
--- a/hal/src/main/native/cpp/jni/HALUtil.cpp
+++ b/hal/src/main/native/cpp/jni/HALUtil.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -73,7 +73,7 @@
      &canNotInitializedExCls},
     {"edu/wpi/first/hal/util/UncleanStatusException", &uncleanStatusExCls}};
 
-namespace frc {
+namespace hal {
 
 void ThrowUncleanStatusException(JNIEnv* env, wpi::StringRef msg,
                                  int32_t status) {
@@ -304,14 +304,14 @@
 
 JavaVM* GetJVM() { return jvm; }
 
-}  // namespace frc
-
 namespace sim {
 jint SimOnLoad(JavaVM* vm, void* reserved);
 void SimOnUnload(JavaVM* vm, void* reserved);
 }  // namespace sim
 
-using namespace frc;
+}  // namespace hal
+
+using namespace hal;
 
 extern "C" {
 

hal/src/main/native/cpp/jni/HALUtil.h

diff --git a/hal/src/main/native/cpp/jni/HALUtil.h b/hal/src/main/native/cpp/jni/HALUtil.h
index c035f75..0a192da 100644
--- a/hal/src/main/native/cpp/jni/HALUtil.h
+++ b/hal/src/main/native/cpp/jni/HALUtil.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,7 +16,7 @@
 struct HAL_MatchInfo;
 struct HAL_Value;
 
-namespace frc {
+namespace hal {
 
 void ReportError(JNIEnv* env, int32_t status, bool doThrow = true);
 
@@ -72,6 +72,6 @@
 
 JavaVM* GetJVM();
 
-}  // namespace frc
+}  // namespace hal
 
 #endif  // HAL_HAL_SRC_MAIN_NATIVE_CPP_JNI_HALUTIL_H_

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

diff --git a/hal/src/main/native/cpp/jni/I2CJNI.cpp b/hal/src/main/native/cpp/jni/I2CJNI.cpp
index 7812bdb..de27df6 100644
--- a/hal/src/main/native/cpp/jni/I2CJNI.cpp
+++ b/hal/src/main/native/cpp/jni/I2CJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,7 +15,7 @@
 #include "edu_wpi_first_hal_I2CJNI.h"
 #include "hal/I2C.h"
 
-using namespace frc;
+using namespace hal;
 using namespace wpi::java;
 
 extern "C" {

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

diff --git a/hal/src/main/native/cpp/jni/InterruptJNI.cpp b/hal/src/main/native/cpp/jni/InterruptJNI.cpp
index e3a783d..d18ef27 100644
--- a/hal/src/main/native/cpp/jni/InterruptJNI.cpp
+++ b/hal/src/main/native/cpp/jni/InterruptJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,7 +18,7 @@
 #include "edu_wpi_first_hal_InterruptJNI.h"
 #include "hal/Interrupts.h"
 
-using namespace frc;
+using namespace hal;
 
 // Thread where callbacks are actually performed.
 //
@@ -294,4 +294,19 @@
   CheckStatus(env, status);
 }
 
+/*
+ * Class:     edu_wpi_first_hal_InterruptJNI
+ * Method:    releaseWaitingInterrupt
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_InterruptJNI_releaseWaitingInterrupt
+  (JNIEnv* env, jclass, jint interruptHandle)
+{
+  int32_t status = 0;
+  HAL_ReleaseWaitingInterrupt((HAL_InterruptHandle)interruptHandle, &status);
+
+  CheckStatus(env, status);
+}
+
 }  // extern "C"

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

diff --git a/hal/src/main/native/cpp/jni/NotifierJNI.cpp b/hal/src/main/native/cpp/jni/NotifierJNI.cpp
index 874d750..ed0b324 100644
--- a/hal/src/main/native/cpp/jni/NotifierJNI.cpp
+++ b/hal/src/main/native/cpp/jni/NotifierJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,7 +16,7 @@
 #include "edu_wpi_first_hal_NotifierJNI.h"
 #include "hal/Notifier.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/PDPJNI.cpp b/hal/src/main/native/cpp/jni/PDPJNI.cpp
index f649a8d..bb28fdf 100644
--- a/hal/src/main/native/cpp/jni/PDPJNI.cpp
+++ b/hal/src/main/native/cpp/jni/PDPJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,7 @@
 #include "hal/PDP.h"
 #include "hal/Ports.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/PWMJNI.cpp b/hal/src/main/native/cpp/jni/PWMJNI.cpp
index 80293c4..085cd0c 100644
--- a/hal/src/main/native/cpp/jni/PWMJNI.cpp
+++ b/hal/src/main/native/cpp/jni/PWMJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,7 +16,7 @@
 #include "hal/Ports.h"
 #include "hal/handles/HandlesInternal.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/PortsJNI.cpp b/hal/src/main/native/cpp/jni/PortsJNI.cpp
index 1adb7fb..65fabd4 100644
--- a/hal/src/main/native/cpp/jni/PortsJNI.cpp
+++ b/hal/src/main/native/cpp/jni/PortsJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 #include "edu_wpi_first_hal_PortsJNI.h"
 #include "hal/Ports.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 /*

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

diff --git a/hal/src/main/native/cpp/jni/PowerJNI.cpp b/hal/src/main/native/cpp/jni/PowerJNI.cpp
index 9184e9e..6fc42b4 100644
--- a/hal/src/main/native/cpp/jni/PowerJNI.cpp
+++ b/hal/src/main/native/cpp/jni/PowerJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 #include "edu_wpi_first_hal_PowerJNI.h"
 #include "hal/Power.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/RelayJNI.cpp b/hal/src/main/native/cpp/jni/RelayJNI.cpp
index c058435..ce43307 100644
--- a/hal/src/main/native/cpp/jni/RelayJNI.cpp
+++ b/hal/src/main/native/cpp/jni/RelayJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,7 +15,7 @@
 #include "hal/Relay.h"
 #include "hal/handles/HandlesInternal.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/SPIJNI.cpp b/hal/src/main/native/cpp/jni/SPIJNI.cpp
index 7962e21..d7709e2 100644
--- a/hal/src/main/native/cpp/jni/SPIJNI.cpp
+++ b/hal/src/main/native/cpp/jni/SPIJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,7 +15,7 @@
 #include "edu_wpi_first_hal_SPIJNI.h"
 #include "hal/SPI.h"
 
-using namespace frc;
+using namespace hal;
 using namespace wpi::java;
 
 extern "C" {

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

diff --git a/hal/src/main/native/cpp/jni/SerialPortJNI.cpp b/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
index 9fe9d92..13f1f99 100644
--- a/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
+++ b/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,7 +15,7 @@
 #include "edu_wpi_first_hal_SerialPortJNI.h"
 #include "hal/SerialPort.h"
 
-using namespace frc;
+using namespace hal;
 using namespace wpi::java;
 
 extern "C" {

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

diff --git a/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp b/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
index d9652cc..b69a8bb 100644
--- a/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
+++ b/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -114,7 +114,7 @@
 Java_edu_wpi_first_hal_SimDeviceJNI_getSimValue
   (JNIEnv* env, jclass, jint handle)
 {
-  return frc::CreateHALValue(env, HAL_GetSimValue(handle));
+  return hal::CreateHALValue(env, HAL_GetSimValue(handle));
 }
 
 /*

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

diff --git a/hal/src/main/native/cpp/jni/SolenoidJNI.cpp b/hal/src/main/native/cpp/jni/SolenoidJNI.cpp
index 4e6f139..4877db3 100644
--- a/hal/src/main/native/cpp/jni/SolenoidJNI.cpp
+++ b/hal/src/main/native/cpp/jni/SolenoidJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 #include "hal/Solenoid.h"
 #include "hal/handles/HandlesInternal.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 

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

diff --git a/hal/src/main/native/cpp/jni/ThreadsJNI.cpp b/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
index d4620e2..a26a4bf 100644
--- a/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
+++ b/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 #include "edu_wpi_first_hal_ThreadsJNI.h"
 #include "hal/Threads.h"
 
-using namespace frc;
+using namespace hal;
 
 extern "C" {
 /*

hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp
new file mode 100644
index 0000000..21d93fe
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp
@@ -0,0 +1,281 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_AccelerometerDataJNI.h"
+#include "hal/simulation/AccelerometerData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    registerActiveCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_registerActiveCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAccelerometerActiveCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    cancelActiveCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_cancelActiveCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAccelerometerActiveCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    getActive
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_getActive
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAccelerometerActive(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    setActive
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_setActive
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAccelerometerActive(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    registerRangeCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_registerRangeCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAccelerometerRangeCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    cancelRangeCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_cancelRangeCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAccelerometerRangeCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    getRange
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_getRange
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAccelerometerRange(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    setRange
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_setRange
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetAccelerometerRange(index,
+                               static_cast<HAL_AccelerometerRange>(value));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    registerXCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_registerXCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAccelerometerXCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    cancelXCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_cancelXCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAccelerometerXCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    getX
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_getX
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAccelerometerX(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    setX
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_setX
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAccelerometerX(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    registerYCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_registerYCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAccelerometerYCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    cancelYCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_cancelYCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAccelerometerYCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    getY
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_getY
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAccelerometerY(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    setY
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_setY
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAccelerometerY(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    registerZCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_registerZCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAccelerometerZCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    cancelZCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_cancelZCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAccelerometerZCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    getZ
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_getZ
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAccelerometerZ(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    setZ
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_setZ
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAccelerometerZ(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AccelerometerDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AccelerometerDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetAccelerometerData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp
new file mode 100644
index 0000000..8ac59ec
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp
@@ -0,0 +1,306 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "edu_wpi_first_hal_simulation_AddressableLEDDataJNI.h"
+#include "hal/simulation/AddressableLEDData.h"
+
+static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));
+
+using namespace hal;
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAddressableLEDInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAddressableLEDInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAddressableLEDInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAddressableLEDInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    registerOutputPortCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_registerOutputPortCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAddressableLEDOutputPortCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    cancelOutputPortCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_cancelOutputPortCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAddressableLEDOutputPortCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    getOutputPort
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_getOutputPort
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAddressableLEDOutputPort(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    setOutputPort
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_setOutputPort
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetAddressableLEDOutputPort(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    registerLengthCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_registerLengthCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAddressableLEDLengthCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    cancelLengthCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_cancelLengthCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAddressableLEDLengthCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    getLength
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_getLength
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAddressableLEDLength(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    setLength
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_setLength
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetAddressableLEDLength(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    registerRunningCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_registerRunningCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAddressableLEDRunningCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    cancelRunningCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_cancelRunningCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAddressableLEDRunningCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    getRunning
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_getRunning
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAddressableLEDRunning(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    setRunning
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_setRunning
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAddressableLEDRunning(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    registerDataCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_registerDataCallback
+  (JNIEnv* env, jclass, jint index, jobject callback)
+{
+  return sim::AllocateConstBufferCallback(
+      env, index, callback, &HALSIM_RegisterAddressableLEDDataCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    cancelDataCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_cancelDataCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  sim::FreeConstBufferCallback(env, handle, index,
+                               &HALSIM_CancelAddressableLEDDataCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    getData
+ * Signature: (I)[B
+ */
+JNIEXPORT jbyteArray JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_getData
+  (JNIEnv* env, jclass, jint index)
+{
+  auto data =
+      std::make_unique<HAL_AddressableLEDData[]>(HAL_kAddressableLEDMaxLength);
+  int32_t length = HALSIM_GetAddressableLEDData(index, data.get());
+  return MakeJByteArray(
+      env, wpi::ArrayRef(reinterpret_cast<jbyte*>(data.get()), length * 4));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    setData
+ * Signature: (I[B)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_setData
+  (JNIEnv* env, jclass, jint index, jbyteArray arr)
+{
+  JByteArrayRef jArrRef{env, arr};
+  auto arrRef = jArrRef.array();
+  HALSIM_SetAddressableLEDData(
+      index, reinterpret_cast<const HAL_AddressableLEDData*>(arrRef.data()),
+      arrRef.size() / 4);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetAddressableLEDData(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AddressableLEDDataJNI
+ * Method:    findForChannel
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AddressableLEDDataJNI_findForChannel
+  (JNIEnv*, jclass, jint channel)
+{
+  return HALSIM_FindAddressableLEDForChannel(channel);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp
new file mode 100644
index 0000000..ed867cd
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp
@@ -0,0 +1,180 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_AnalogGyroDataJNI.h"
+#include "hal/simulation/AnalogGyroData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    registerAngleCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_registerAngleCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogGyroAngleCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    cancelAngleCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_cancelAngleCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogGyroAngleCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    getAngle
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_getAngle
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogGyroAngle(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    setAngle
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_setAngle
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAnalogGyroAngle(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    registerRateCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_registerRateCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogGyroRateCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    cancelRateCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_cancelRateCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogGyroRateCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    getRate
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_getRate
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogGyroRate(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    setRate
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_setRate
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAnalogGyroRate(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogGyroInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogGyroInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogGyroInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAnalogGyroInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogGyroDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogGyroDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetAnalogGyroData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp
new file mode 100644
index 0000000..0266a76
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp
@@ -0,0 +1,485 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_AnalogInDataJNI.h"
+#include "hal/simulation/AnalogInData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogInInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAnalogInInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerAverageBitsCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerAverageBitsCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogInAverageBitsCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelAverageBitsCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelAverageBitsCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInAverageBitsCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getAverageBits
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getAverageBits
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInAverageBits(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setAverageBits
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setAverageBits
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetAnalogInAverageBits(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerOversampleBitsCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerOversampleBitsCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogInOversampleBitsCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelOversampleBitsCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelOversampleBitsCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInOversampleBitsCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getOversampleBits
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getOversampleBits
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInOversampleBits(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setOversampleBits
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setOversampleBits
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetAnalogInOversampleBits(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerVoltageCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogInVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelVoltageCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getVoltage
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInVoltage(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setVoltage
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAnalogInVoltage(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerAccumulatorInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerAccumulatorInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogInAccumulatorInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelAccumulatorInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelAccumulatorInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(
+      env, handle, index, &HALSIM_CancelAnalogInAccumulatorInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getAccumulatorInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getAccumulatorInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInAccumulatorInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setAccumulatorInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setAccumulatorInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAnalogInAccumulatorInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerAccumulatorValueCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerAccumulatorValueCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogInAccumulatorValueCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelAccumulatorValueCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelAccumulatorValueCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInAccumulatorValueCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getAccumulatorValue
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getAccumulatorValue
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInAccumulatorValue(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setAccumulatorValue
+ * Signature: (IJ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setAccumulatorValue
+  (JNIEnv*, jclass, jint index, jlong value)
+{
+  HALSIM_SetAnalogInAccumulatorValue(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerAccumulatorCountCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerAccumulatorCountCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogInAccumulatorCountCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelAccumulatorCountCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelAccumulatorCountCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInAccumulatorCountCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getAccumulatorCount
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getAccumulatorCount
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInAccumulatorCount(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setAccumulatorCount
+ * Signature: (IJ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setAccumulatorCount
+  (JNIEnv*, jclass, jint index, jlong value)
+{
+  HALSIM_SetAnalogInAccumulatorCount(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerAccumulatorCenterCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerAccumulatorCenterCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogInAccumulatorCenterCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelAccumulatorCenterCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelAccumulatorCenterCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInAccumulatorCenterCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getAccumulatorCenter
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getAccumulatorCenter
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInAccumulatorCenter(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setAccumulatorCenter
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setAccumulatorCenter
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetAnalogInAccumulatorCenter(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    registerAccumulatorDeadbandCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_registerAccumulatorDeadbandCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogInAccumulatorDeadbandCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    cancelAccumulatorDeadbandCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_cancelAccumulatorDeadbandCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogInAccumulatorDeadbandCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    getAccumulatorDeadband
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_getAccumulatorDeadband
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogInAccumulatorDeadband(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    setAccumulatorDeadband
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_setAccumulatorDeadband
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetAnalogInAccumulatorDeadband(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogInDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogInDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetAnalogInData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp
new file mode 100644
index 0000000..06f5be6
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_AnalogOutDataJNI.h"
+#include "hal/simulation/AnalogOutData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    registerVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_registerVoltageCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogOutVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    cancelVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_cancelVoltageCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogOutVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_getVoltage
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogOutVoltage(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    setVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_setVoltage
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAnalogOutVoltage(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterAnalogOutInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogOutInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogOutInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAnalogOutInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogOutDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogOutDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetAnalogOutData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp
new file mode 100644
index 0000000..be92c6d
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_AnalogTriggerDataJNI.h"
+#include "hal/simulation/AnalogTriggerData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogTriggerInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelAnalogTriggerInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogTriggerInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetAnalogTriggerInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    registerTriggerLowerBoundCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_registerTriggerLowerBoundCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    cancelTriggerLowerBoundCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_cancelTriggerLowerBoundCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(
+      env, handle, index, &HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    getTriggerLowerBound
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_getTriggerLowerBound
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogTriggerTriggerLowerBound(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    setTriggerLowerBound
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_setTriggerLowerBound
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAnalogTriggerTriggerLowerBound(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    registerTriggerUpperBoundCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_registerTriggerUpperBoundCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    cancelTriggerUpperBoundCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_cancelTriggerUpperBoundCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(
+      env, handle, index, &HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    getTriggerUpperBound
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_getTriggerUpperBound
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetAnalogTriggerTriggerUpperBound(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    setTriggerUpperBound
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_setTriggerUpperBound
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetAnalogTriggerTriggerUpperBound(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetAnalogTriggerData(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_AnalogTriggerDataJNI
+ * Method:    findForChannel
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_AnalogTriggerDataJNI_findForChannel
+  (JNIEnv*, jclass, jint channel)
+{
+  return HALSIM_FindAnalogTriggerForChannel(channel);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp b/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp
new file mode 100644
index 0000000..6751e03
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "BufferCallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+
+using namespace hal;
+using namespace hal::sim;
+using namespace wpi::java;
+
+static hal::UnlimitedHandleResource<SIM_JniHandle, BufferCallbackStore,
+                                    hal::HAL_HandleEnum::SimulationJni>*
+    callbackHandles;
+
+namespace hal {
+namespace sim {
+void InitializeBufferStore() {
+  static hal::UnlimitedHandleResource<SIM_JniHandle, BufferCallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cb;
+  callbackHandles = &cb;
+}
+}  // namespace sim
+}  // namespace hal
+
+void BufferCallbackStore::create(JNIEnv* env, jobject obj) {
+  m_call = JGlobal<jobject>(env, obj);
+}
+
+void BufferCallbackStore::performCallback(const char* name, uint8_t* buffer,
+                                          uint32_t length) {
+  JNIEnv* env;
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      wpi::outs() << "Failed to attach\n";
+      wpi::outs().flush();
+      return;
+    }
+  } else if (tryGetEnv == JNI_EVERSION) {
+    wpi::outs() << "Invalid JVM Version requested\n";
+    wpi::outs().flush();
+  }
+
+  auto toCallbackArr =
+      MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
+                                         static_cast<size_t>(length)});
+
+  env->CallVoidMethod(m_call, sim::GetBufferCallback(), MakeJString(env, name),
+                      toCallbackArr, (jint)length);
+
+  jbyte* fromCallbackArr = reinterpret_cast<jbyte*>(
+      env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
+
+  for (size_t i = 0; i < length; i++) {
+    buffer[i] = fromCallbackArr[i];
+  }
+
+  env->ReleasePrimitiveArrayCritical(toCallbackArr, fromCallbackArr, JNI_ABORT);
+
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
+}
+
+void BufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
+
+SIM_JniHandle sim::AllocateBufferCallback(
+    JNIEnv* env, jint index, jobject callback,
+    RegisterBufferCallbackFunc createCallback) {
+  auto callbackStore = std::make_shared<BufferCallbackStore>();
+
+  auto handle = callbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param, uint8_t* buffer,
+                         uint32_t length) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = callbackHandles->Get(handle);
+    if (!data) return;
+
+    data->performCallback(name, buffer, length);
+  };
+
+  auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
+}
+
+void sim::FreeBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                             FreeBufferCallbackFunc freeCallback) {
+  auto callback = callbackHandles->Free(handle);
+  freeCallback(index, callback->getCallbackId());
+  callback->free(env);
+}

hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h

diff --git a/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h b/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h
new file mode 100644
index 0000000..8e746d2
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+
+namespace hal {
+namespace sim {
+class BufferCallbackStore {
+ public:
+  void create(JNIEnv* env, jobject obj);
+  void performCallback(const char* name, uint8_t* buffer, uint32_t length);
+  void free(JNIEnv* env);
+  void setCallbackId(int32_t id) { callbackId = id; }
+  int32_t getCallbackId() { return callbackId; }
+
+ private:
+  wpi::java::JGlobal<jobject> m_call;
+  int32_t callbackId;
+};
+
+void InitializeBufferStore();
+
+typedef int32_t (*RegisterBufferCallbackFunc)(int32_t index,
+                                              HAL_BufferCallback callback,
+                                              void* param);
+typedef void (*FreeBufferCallbackFunc)(int32_t index, int32_t uid);
+
+SIM_JniHandle AllocateBufferCallback(JNIEnv* env, jint index, jobject callback,
+                                     RegisterBufferCallbackFunc createCallback);
+void FreeBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                        FreeBufferCallbackFunc freeCallback);
+}  // namespace sim
+}  // namespace hal

hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp b/hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp
new file mode 100644
index 0000000..5ce046d
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp
@@ -0,0 +1,197 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+
+using namespace hal;
+using namespace hal::sim;
+using namespace wpi::java;
+
+static hal::UnlimitedHandleResource<SIM_JniHandle, CallbackStore,
+                                    hal::HAL_HandleEnum::SimulationJni>*
+    callbackHandles;
+
+namespace hal {
+namespace sim {
+void InitializeStore() {
+  static hal::UnlimitedHandleResource<SIM_JniHandle, CallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cb;
+  callbackHandles = &cb;
+}
+}  // namespace sim
+}  // namespace hal
+
+void CallbackStore::create(JNIEnv* env, jobject obj) {
+  m_call = JGlobal<jobject>(env, obj);
+}
+
+void CallbackStore::performCallback(const char* name, const HAL_Value* value) {
+  JNIEnv* env;
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      wpi::outs() << "Failed to attach\n";
+      wpi::outs().flush();
+      return;
+    }
+  } else if (tryGetEnv == JNI_EVERSION) {
+    wpi::outs() << "Invalid JVM Version requested\n";
+    wpi::outs().flush();
+  }
+
+  env->CallVoidMethod(m_call, sim::GetNotifyCallback(), MakeJString(env, name),
+                      (jint)value->type, (jlong)value->data.v_long,
+                      (jdouble)value->data.v_double);
+
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
+}
+
+void CallbackStore::free(JNIEnv* env) { m_call.free(env); }
+
+SIM_JniHandle sim::AllocateCallback(JNIEnv* env, jint index, jobject callback,
+                                    jboolean initialNotify,
+                                    RegisterCallbackFunc createCallback) {
+  auto callbackStore = std::make_shared<CallbackStore>();
+
+  auto handle = callbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param,
+                         const HAL_Value* value) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = callbackHandles->Get(handle);
+    if (!data) return;
+
+    data->performCallback(name, value);
+  };
+
+  auto id = createCallback(index, callbackFunc, handleAsVoidPtr, initialNotify);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
+}
+
+void sim::FreeCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                       FreeCallbackFunc freeCallback) {
+  auto callback = callbackHandles->Free(handle);
+  freeCallback(index, callback->getCallbackId());
+  callback->free(env);
+}
+
+SIM_JniHandle sim::AllocateChannelCallback(
+    JNIEnv* env, jint index, jint channel, jobject callback,
+    jboolean initialNotify, RegisterChannelCallbackFunc createCallback) {
+  auto callbackStore = std::make_shared<CallbackStore>();
+
+  auto handle = callbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param,
+                         const HAL_Value* value) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = callbackHandles->Get(handle);
+    if (!data) return;
+
+    data->performCallback(name, value);
+  };
+
+  auto id = createCallback(index, channel, callbackFunc, handleAsVoidPtr,
+                           initialNotify);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
+}
+
+void sim::FreeChannelCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                              jint channel,
+                              FreeChannelCallbackFunc freeCallback) {
+  auto callback = callbackHandles->Free(handle);
+  freeCallback(index, channel, callback->getCallbackId());
+  callback->free(env);
+}
+
+SIM_JniHandle sim::AllocateCallbackNoIndex(
+    JNIEnv* env, jobject callback, jboolean initialNotify,
+    RegisterCallbackNoIndexFunc createCallback) {
+  auto callbackStore = std::make_shared<CallbackStore>();
+
+  auto handle = callbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param,
+                         const HAL_Value* value) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = callbackHandles->Get(handle);
+    if (!data) return;
+
+    data->performCallback(name, value);
+  };
+
+  auto id = createCallback(callbackFunc, handleAsVoidPtr, initialNotify);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
+}
+
+void sim::FreeCallbackNoIndex(JNIEnv* env, SIM_JniHandle handle,
+                              FreeCallbackNoIndexFunc freeCallback) {
+  auto callback = callbackHandles->Free(handle);
+  freeCallback(callback->getCallbackId());
+  callback->free(env);
+}

hal/src/main/native/cpp/jni/simulation/CallbackStore.h

diff --git a/hal/src/main/native/cpp/jni/simulation/CallbackStore.h b/hal/src/main/native/cpp/jni/simulation/CallbackStore.h
new file mode 100644
index 0000000..0ba44d3
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/CallbackStore.h
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+
+namespace hal {
+namespace sim {
+class CallbackStore {
+ public:
+  void create(JNIEnv* env, jobject obj);
+  void performCallback(const char* name, const HAL_Value* value);
+  void free(JNIEnv* env);
+  void setCallbackId(int32_t id) { callbackId = id; }
+  int32_t getCallbackId() { return callbackId; }
+
+ private:
+  wpi::java::JGlobal<jobject> m_call;
+  int32_t callbackId;
+};
+
+void InitializeStore();
+
+typedef int32_t (*RegisterCallbackFunc)(int32_t index,
+                                        HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
+typedef void (*FreeCallbackFunc)(int32_t index, int32_t uid);
+typedef int32_t (*RegisterChannelCallbackFunc)(int32_t index, int32_t channel,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+typedef void (*FreeChannelCallbackFunc)(int32_t index, int32_t channel,
+                                        int32_t uid);
+typedef int32_t (*RegisterCallbackNoIndexFunc)(HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+typedef void (*FreeCallbackNoIndexFunc)(int32_t uid);
+
+SIM_JniHandle AllocateCallback(JNIEnv* env, jint index, jobject callback,
+                               jboolean initialNotify,
+                               RegisterCallbackFunc createCallback);
+SIM_JniHandle AllocateChannelCallback(
+    JNIEnv* env, jint index, jint channel, jobject callback,
+    jboolean initialNotify, RegisterChannelCallbackFunc createCallback);
+SIM_JniHandle AllocateCallbackNoIndex(
+    JNIEnv* env, jobject callback, jboolean initialNotify,
+    RegisterCallbackNoIndexFunc createCallback);
+void FreeCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                  FreeCallbackFunc freeCallback);
+void FreeChannelCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                         jint channel, FreeChannelCallbackFunc freeCallback);
+void FreeCallbackNoIndex(JNIEnv* env, SIM_JniHandle handle,
+                         FreeCallbackNoIndexFunc freeCallback);
+}  // namespace sim
+}  // namespace hal

hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp b/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp
new file mode 100644
index 0000000..b9c9808
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ConstBufferCallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+
+using namespace hal;
+using namespace hal::sim;
+using namespace wpi::java;
+
+static hal::UnlimitedHandleResource<SIM_JniHandle, ConstBufferCallbackStore,
+                                    hal::HAL_HandleEnum::SimulationJni>*
+    callbackHandles;
+
+namespace hal {
+namespace sim {
+void InitializeConstBufferStore() {
+  static hal::UnlimitedHandleResource<SIM_JniHandle, ConstBufferCallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cb;
+  callbackHandles = &cb;
+}
+}  // namespace sim
+}  // namespace hal
+
+void ConstBufferCallbackStore::create(JNIEnv* env, jobject obj) {
+  m_call = JGlobal<jobject>(env, obj);
+}
+
+void ConstBufferCallbackStore::performCallback(const char* name,
+                                               const uint8_t* buffer,
+                                               uint32_t length) {
+  JNIEnv* env;
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      wpi::outs() << "Failed to attach\n";
+      wpi::outs().flush();
+      return;
+    }
+  } else if (tryGetEnv == JNI_EVERSION) {
+    wpi::outs() << "Invalid JVM Version requested\n";
+    wpi::outs().flush();
+  }
+
+  auto toCallbackArr =
+      MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
+                                         static_cast<size_t>(length)});
+
+  env->CallVoidMethod(m_call, sim::GetConstBufferCallback(),
+                      MakeJString(env, name), toCallbackArr, (jint)length);
+
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
+}
+
+void ConstBufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
+
+SIM_JniHandle sim::AllocateConstBufferCallback(
+    JNIEnv* env, jint index, jobject callback,
+    RegisterConstBufferCallbackFunc createCallback) {
+  auto callbackStore = std::make_shared<ConstBufferCallbackStore>();
+
+  auto handle = callbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param, const uint8_t* buffer,
+                         uint32_t length) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = callbackHandles->Get(handle);
+    if (!data) return;
+
+    data->performCallback(name, buffer, length);
+  };
+
+  auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
+}
+
+void sim::FreeConstBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                                  FreeConstBufferCallbackFunc freeCallback) {
+  auto callback = callbackHandles->Free(handle);
+  freeCallback(index, callback->getCallbackId());
+  callback->free(env);
+}

hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h

diff --git a/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h b/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h
new file mode 100644
index 0000000..b69eccf
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+
+namespace hal {
+namespace sim {
+class ConstBufferCallbackStore {
+ public:
+  void create(JNIEnv* env, jobject obj);
+  void performCallback(const char* name, const uint8_t* buffer,
+                       uint32_t length);
+  void free(JNIEnv* env);
+  void setCallbackId(int32_t id) { callbackId = id; }
+  int32_t getCallbackId() { return callbackId; }
+
+ private:
+  wpi::java::JGlobal<jobject> m_call;
+  int32_t callbackId;
+};
+
+void InitializeConstBufferStore();
+
+typedef int32_t (*RegisterConstBufferCallbackFunc)(
+    int32_t index, HAL_ConstBufferCallback callback, void* param);
+typedef void (*FreeConstBufferCallbackFunc)(int32_t index, int32_t uid);
+
+SIM_JniHandle AllocateConstBufferCallback(
+    JNIEnv* env, jint index, jobject callback,
+    RegisterConstBufferCallbackFunc createCallback);
+void FreeConstBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                             FreeConstBufferCallbackFunc freeCallback);
+}  // namespace sim
+}  // namespace hal

hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp
new file mode 100644
index 0000000..f757558
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp
@@ -0,0 +1,279 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_DIODataJNI.h"
+#include "hal/simulation/DIOData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDIOInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDIOInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDIOInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetDIOInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    registerValueCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_registerValueCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDIOValueCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    cancelValueCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_cancelValueCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index, &HALSIM_CancelDIOValueCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    getValue
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_getValue
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDIOValue(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    setValue
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_setValue
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetDIOValue(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    registerPulseLengthCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_registerPulseLengthCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDIOPulseLengthCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    cancelPulseLengthCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_cancelPulseLengthCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDIOPulseLengthCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    getPulseLength
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_getPulseLength
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDIOPulseLength(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    setPulseLength
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_setPulseLength
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetDIOPulseLength(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    registerIsInputCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_registerIsInputCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDIOIsInputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    cancelIsInputCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_cancelIsInputCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDIOIsInputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    getIsInput
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_getIsInput
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDIOIsInput(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    setIsInput
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_setIsInput
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetDIOIsInput(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    registerFilterIndexCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_registerFilterIndexCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDIOFilterIndexCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    cancelFilterIndexCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_cancelFilterIndexCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDIOFilterIndexCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    getFilterIndex
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_getFilterIndex
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDIOFilterIndex(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    setFilterIndex
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_setFilterIndex
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetDIOFilterIndex(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DIODataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DIODataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetDIOData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp
new file mode 100644
index 0000000..ea6bee2
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp
@@ -0,0 +1,192 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_DigitalPWMDataJNI.h"
+#include "hal/simulation/DigitalPWMData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDigitalPWMInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDigitalPWMInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDigitalPWMInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetDigitalPWMInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    registerDutyCycleCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_registerDutyCycleCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDigitalPWMDutyCycleCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    cancelDutyCycleCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_cancelDutyCycleCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDigitalPWMDutyCycleCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    getDutyCycle
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_getDutyCycle
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDigitalPWMDutyCycle(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    setDutyCycle
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_setDutyCycle
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetDigitalPWMDutyCycle(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    registerPinCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_registerPinCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDigitalPWMPinCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    cancelPinCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_cancelPinCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDigitalPWMPinCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    getPin
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_getPin
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDigitalPWMPin(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    setPin
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_setPin
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetDigitalPWMPin(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetDigitalPWMData(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DigitalPWMDataJNI
+ * Method:    findForChannel
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DigitalPWMDataJNI_findForChannel
+  (JNIEnv*, jclass, jint channel)
+{
+  return HALSIM_FindDigitalPWMForChannel(channel);
+}
+
+}  // extern "C"

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

diff --git a/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
new file mode 100644
index 0000000..7868e43
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
@@ -0,0 +1,825 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include <cstring>
+
+#include <wpi/jni_util.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_DriverStationDataJNI.h"
+#include "hal/simulation/DriverStationData.h"
+#include "hal/simulation/MockHooks.h"
+
+using namespace hal;
+using namespace wpi::java;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerEnabledCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerEnabledCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterDriverStationEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelEnabledCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelEnabledCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelDriverStationEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getEnabled
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getEnabled
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationEnabled();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setEnabled
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setEnabled
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetDriverStationEnabled(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerAutonomousCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerAutonomousCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterDriverStationAutonomousCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelAutonomousCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelAutonomousCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(
+      env, handle, &HALSIM_CancelDriverStationAutonomousCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getAutonomous
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getAutonomous
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationAutonomous();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setAutonomous
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setAutonomous
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetDriverStationAutonomous(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerTestCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerTestCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify, &HALSIM_RegisterDriverStationTestCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelTestCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelTestCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelDriverStationTestCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getTest
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getTest
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationTest();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setTest
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setTest
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetDriverStationTest(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerEStopCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerEStopCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify, &HALSIM_RegisterDriverStationEStopCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelEStopCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelEStopCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelDriverStationEStopCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getEStop
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getEStop
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationEStop();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setEStop
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setEStop
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetDriverStationEStop(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerFmsAttachedCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerFmsAttachedCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterDriverStationFmsAttachedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelFmsAttachedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelFmsAttachedCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(
+      env, handle, &HALSIM_CancelDriverStationFmsAttachedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getFmsAttached
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getFmsAttached
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationFmsAttached();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setFmsAttached
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setFmsAttached
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetDriverStationFmsAttached(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerDsAttachedCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerDsAttachedCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterDriverStationDsAttachedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelDsAttachedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelDsAttachedCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(
+      env, handle, &HALSIM_CancelDriverStationDsAttachedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getDsAttached
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getDsAttached
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationDsAttached();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setDsAttached
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setDsAttached
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetDriverStationDsAttached(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerAllianceStationIdCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerAllianceStationIdCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterDriverStationAllianceStationIdCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelAllianceStationIdCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelAllianceStationIdCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(
+      env, handle, &HALSIM_CancelDriverStationAllianceStationIdCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getAllianceStationId
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getAllianceStationId
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationAllianceStationId();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setAllianceStationId
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setAllianceStationId
+  (JNIEnv*, jclass, jint value)
+{
+  HALSIM_SetDriverStationAllianceStationId(
+      static_cast<HAL_AllianceStationID>(value));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerMatchTimeCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerMatchTimeCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterDriverStationMatchTimeCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    cancelMatchTimeCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_cancelMatchTimeCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelDriverStationMatchTimeCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getMatchTime
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getMatchTime
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetDriverStationMatchTime();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setMatchTime
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setMatchTime
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetDriverStationMatchTime(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickAxes
+ * Signature: (B[F)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickAxes
+  (JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray)
+{
+  HAL_JoystickAxes axes;
+  {
+    wpi::java::JFloatArrayRef jArrayRef(env, axesArray);
+    auto arrayRef = jArrayRef.array();
+    auto arraySize = arrayRef.size();
+    int maxCount =
+        arraySize < HAL_kMaxJoystickAxes ? arraySize : HAL_kMaxJoystickAxes;
+    axes.count = maxCount;
+    for (int i = 0; i < maxCount; i++) {
+      axes.axes[i] = arrayRef[i];
+    }
+  }
+  HALSIM_SetJoystickAxes(joystickNum, &axes);
+  return;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickPOVs
+ * Signature: (B[S)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickPOVs
+  (JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray)
+{
+  HAL_JoystickPOVs povs;
+  {
+    wpi::java::JShortArrayRef jArrayRef(env, povsArray);
+    auto arrayRef = jArrayRef.array();
+    auto arraySize = arrayRef.size();
+    int maxCount =
+        arraySize < HAL_kMaxJoystickPOVs ? arraySize : HAL_kMaxJoystickPOVs;
+    povs.count = maxCount;
+    for (int i = 0; i < maxCount; i++) {
+      povs.povs[i] = arrayRef[i];
+    }
+  }
+  HALSIM_SetJoystickPOVs(joystickNum, &povs);
+  return;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickButtons
+ * Signature: (BII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickButtons
+  (JNIEnv* env, jclass, jbyte joystickNum, jint buttons, jint count)
+{
+  if (count > 32) {
+    count = 32;
+  }
+  HAL_JoystickButtons joystickButtons;
+  joystickButtons.count = count;
+  joystickButtons.buttons = buttons;
+  HALSIM_SetJoystickButtons(joystickNum, &joystickButtons);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getJoystickOutputs
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getJoystickOutputs
+  (JNIEnv* env, jclass, jint stick)
+{
+  int64_t outputs = 0;
+  int32_t leftRumble;
+  int32_t rightRumble;
+  HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+  return outputs;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    getJoystickRumble
+ * Signature: (II)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_getJoystickRumble
+  (JNIEnv* env, jclass, jint stick, jint rumbleNum)
+{
+  int64_t outputs;
+  int32_t leftRumble = 0;
+  int32_t rightRumble = 0;
+  HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+  return rumbleNum == 0 ? leftRumble : rightRumble;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setMatchInfo
+ * Signature: (Ljava/lang/String;Ljava/lang/String;III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setMatchInfo
+  (JNIEnv* env, jclass, jstring eventName, jstring gameSpecificMessage,
+   jint matchNumber, jint replayNumber, jint matchType)
+{
+  JStringRef eventNameRef{env, eventName};
+  JStringRef gameSpecificMessageRef{env, gameSpecificMessage};
+
+  HAL_MatchInfo halMatchInfo;
+  std::snprintf(halMatchInfo.eventName, sizeof(halMatchInfo.eventName), "%s",
+                eventNameRef.c_str());
+  std::snprintf(reinterpret_cast<char*>(halMatchInfo.gameSpecificMessage),
+                sizeof(halMatchInfo.gameSpecificMessage), "%s",
+                gameSpecificMessageRef.c_str());
+  halMatchInfo.gameSpecificMessageSize = gameSpecificMessageRef.size();
+  halMatchInfo.matchType = (HAL_MatchType)matchType;
+  halMatchInfo.matchNumber = matchNumber;
+  halMatchInfo.replayNumber = replayNumber;
+  HALSIM_SetMatchInfo(&halMatchInfo);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    registerAllCallbacks
+ * Signature: (Ljava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_registerAllCallbacks
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      [](HAL_NotifyCallback cb, void* param, HAL_Bool in) {
+        HALSIM_RegisterDriverStationAllCallbacks(cb, param, in);
+        return 0;
+      });
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    notifyNewData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_notifyNewData
+  (JNIEnv*, jclass)
+{
+  HALSIM_NotifyDriverStationNewData();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setSendError
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setSendError
+  (JNIEnv*, jclass, jboolean shouldSend)
+{
+  if (shouldSend) {
+    HALSIM_SetSendError(nullptr);
+  } else {
+    HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
+                           HAL_Bool isLVCode, const char* details,
+                           const char* location, const char* callStack,
+                           HAL_Bool printMsg) { return 0; });
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setSendConsoleLine
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setSendConsoleLine
+  (JNIEnv*, jclass, jboolean shouldSend)
+{
+  if (shouldSend) {
+    HALSIM_SetSendConsoleLine(nullptr);
+  } else {
+    HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickButton
+ * Signature: (IIZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickButton
+  (JNIEnv*, jclass, jint stick, jint button, jboolean state)
+{
+  HALSIM_SetJoystickButton(stick, button, state);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickAxis
+ * Signature: (IID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickAxis
+  (JNIEnv*, jclass, jint stick, jint axis, jdouble value)
+{
+  HALSIM_SetJoystickAxis(stick, axis, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickPOV
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickPOV
+  (JNIEnv*, jclass, jint stick, jint pov, jint value)
+{
+  HALSIM_SetJoystickPOV(stick, pov, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickButtonsValue
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickButtonsValue
+  (JNIEnv*, jclass, jint stick, jint buttons)
+{
+  HALSIM_SetJoystickButtonsValue(stick, buttons);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickAxisCount
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickAxisCount
+  (JNIEnv*, jclass, jint stick, jint count)
+{
+  HALSIM_SetJoystickAxisCount(stick, count);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickPOVCount
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickPOVCount
+  (JNIEnv*, jclass, jint stick, jint count)
+{
+  HALSIM_SetJoystickPOVCount(stick, count);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickButtonCount
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickButtonCount
+  (JNIEnv*, jclass, jint stick, jint count)
+{
+  HALSIM_SetJoystickButtonCount(stick, count);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickIsXbox
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickIsXbox
+  (JNIEnv*, jclass, jint stick, jboolean isXbox)
+{
+  HALSIM_SetJoystickIsXbox(stick, isXbox);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickType
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickType
+  (JNIEnv*, jclass, jint stick, jint type)
+{
+  HALSIM_SetJoystickType(stick, type);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickName
+ * Signature: (ILjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickName
+  (JNIEnv* env, jclass, jint stick, jstring name)
+{
+  HALSIM_SetJoystickName(stick, JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setJoystickAxisType
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setJoystickAxisType
+  (JNIEnv*, jclass, jint stick, jint axis, jint type)
+{
+  HALSIM_SetJoystickAxisType(stick, axis, type);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setGameSpecificMessage
+ * Signature: (Ljava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setGameSpecificMessage
+  (JNIEnv* env, jclass, jstring message)
+{
+  HALSIM_SetGameSpecificMessage(JStringRef{env, message}.c_str());
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setEventName
+ * Signature: (Ljava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setEventName
+  (JNIEnv* env, jclass, jstring name)
+{
+  HALSIM_SetEventName(JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setMatchType
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setMatchType
+  (JNIEnv*, jclass, jint type)
+{
+  HALSIM_SetMatchType(static_cast<HAL_MatchType>(static_cast<int>(type)));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setMatchNumber
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setMatchNumber
+  (JNIEnv*, jclass, jint matchNumber)
+{
+  HALSIM_SetMatchNumber(matchNumber);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    setReplayNumber
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_setReplayNumber
+  (JNIEnv*, jclass, jint replayNumber)
+{
+  HALSIM_SetReplayNumber(replayNumber);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DriverStationDataJNI
+ * Method:    resetData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DriverStationDataJNI_resetData
+  (JNIEnv*, jclass)
+{
+  HALSIM_ResetDriverStationData();
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp
new file mode 100644
index 0000000..054fa67
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp
@@ -0,0 +1,192 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_DutyCycleDataJNI.h"
+#include "hal/simulation/DutyCycleData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDutyCycleInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDutyCycleInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDutyCycleInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetDutyCycleInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    registerFrequencyCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_registerFrequencyCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDutyCycleFrequencyCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    cancelFrequencyCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_cancelFrequencyCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDutyCycleFrequencyCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    getFrequency
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_getFrequency
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDutyCycleFrequency(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    setFrequency
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_setFrequency
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetDutyCycleFrequency(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    registerOutputCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_registerOutputCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterDutyCycleOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    cancelOutputCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_cancelOutputCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelDutyCycleOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    getOutput
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_getOutput
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetDutyCycleOutput(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    setOutput
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_setOutput
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetDutyCycleOutput(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetDutyCycleData(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_DutyCycleDataJNI
+ * Method:    findForChannel
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_DutyCycleDataJNI_findForChannel
+  (JNIEnv*, jclass, jint channel)
+{
+  return HALSIM_FindDutyCycleForChannel(channel);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp
new file mode 100644
index 0000000..b97e0fe
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp
@@ -0,0 +1,490 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_EncoderDataJNI.h"
+#include "hal/simulation/EncoderData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetEncoderInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerCountCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerCountCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderCountCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelCountCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelCountCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderCountCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getCount
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getCount
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderCount(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setCount
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setCount
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetEncoderCount(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerPeriodCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerPeriodCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderPeriodCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelPeriodCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelPeriodCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderPeriodCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getPeriod
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getPeriod
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderPeriod(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setPeriod
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setPeriod
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetEncoderPeriod(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerResetCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerResetCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderResetCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelResetCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelResetCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderResetCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getReset
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getReset
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderReset(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setReset
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setReset
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetEncoderReset(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerMaxPeriodCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerMaxPeriodCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderMaxPeriodCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelMaxPeriodCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelMaxPeriodCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderMaxPeriodCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getMaxPeriod
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getMaxPeriod
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderMaxPeriod(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setMaxPeriod
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setMaxPeriod
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetEncoderMaxPeriod(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerDirectionCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerDirectionCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderDirectionCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelDirectionCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelDirectionCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderDirectionCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getDirection
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getDirection
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderDirection(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setDirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setDirection
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetEncoderDirection(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerReverseDirectionCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerReverseDirectionCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderReverseDirectionCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelReverseDirectionCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelReverseDirectionCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderReverseDirectionCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getReverseDirection
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getReverseDirection
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderReverseDirection(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setReverseDirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setReverseDirection
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetEncoderReverseDirection(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    registerSamplesToAverageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_registerSamplesToAverageCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterEncoderSamplesToAverageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    cancelSamplesToAverageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_cancelSamplesToAverageCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelEncoderSamplesToAverageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getSamplesToAverage
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getSamplesToAverage
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderSamplesToAverage(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setSamplesToAverage
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setSamplesToAverage
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetEncoderSamplesToAverage(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setDistance
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setDistance
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetEncoderDistance(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getDistance
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getDistance
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderDistance(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    setRate
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_setRate
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetEncoderRate(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    getRate
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_getRate
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetEncoderRate(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetEncoderData(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_EncoderDataJNI
+ * Method:    findForChannel
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_EncoderDataJNI_findForChannel
+  (JNIEnv*, jclass, jint channel)
+{
+  return HALSIM_FindEncoderForChannel(channel);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp
new file mode 100644
index 0000000..e4c2259
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "BufferCallbackStore.h"
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "edu_wpi_first_hal_simulation_I2CDataJNI.h"
+#include "hal/simulation/I2CData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterI2CInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelI2CInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetI2CInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetI2CInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    registerReadCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_registerReadCallback
+  (JNIEnv* env, jclass, jint index, jobject callback)
+{
+  return sim::AllocateBufferCallback(env, index, callback,
+                                     &HALSIM_RegisterI2CReadCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    cancelReadCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_cancelReadCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  sim::FreeBufferCallback(env, handle, index, &HALSIM_CancelI2CReadCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    registerWriteCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_registerWriteCallback
+  (JNIEnv* env, jclass, jint index, jobject callback)
+{
+  return sim::AllocateConstBufferCallback(env, index, callback,
+                                          &HALSIM_RegisterI2CWriteCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    cancelWriteCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_cancelWriteCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  sim::FreeConstBufferCallback(env, handle, index,
+                               &HALSIM_CancelI2CWriteCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_I2CDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_I2CDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetI2CData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp
new file mode 100644
index 0000000..845c164
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "edu_wpi_first_hal_simulation_NotifierDataJNI.h"
+#include "hal/simulation/NotifierData.h"
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_NotifierDataJNI
+ * Method:    getNextTimeout
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_simulation_NotifierDataJNI_getNextTimeout
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetNextNotifierTimeout();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_NotifierDataJNI
+ * Method:    getNumNotifiers
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_NotifierDataJNI_getNumNotifiers
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetNumNotifiers();
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/PCMDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/PCMDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/PCMDataJNI.cpp
new file mode 100644
index 0000000..cfcd2c0
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/PCMDataJNI.cpp
@@ -0,0 +1,421 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_PCMDataJNI.h"
+#include "hal/simulation/PCMData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerSolenoidInitializedCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerSolenoidInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  return sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      &HALSIM_RegisterPCMSolenoidInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    cancelSolenoidInitializedCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelSolenoidInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+  return sim::FreeChannelCallback(env, handle, index, channel,
+                                  &HALSIM_CancelPCMSolenoidInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    getSolenoidInitialized
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_getSolenoidInitialized
+  (JNIEnv*, jclass, jint index, jint channel)
+{
+  return HALSIM_GetPCMSolenoidInitialized(index, channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    setSolenoidInitialized
+ * Signature: (IIZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_setSolenoidInitialized
+  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
+{
+  HALSIM_SetPCMSolenoidInitialized(index, channel, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerSolenoidOutputCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerSolenoidOutputCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  return sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      &HALSIM_RegisterPCMSolenoidOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    cancelSolenoidOutputCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelSolenoidOutputCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+  return sim::FreeChannelCallback(env, handle, index, channel,
+                                  &HALSIM_CancelPCMSolenoidOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    getSolenoidOutput
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_getSolenoidOutput
+  (JNIEnv*, jclass, jint index, jint channel)
+{
+  return HALSIM_GetPCMSolenoidOutput(index, channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    setSolenoidOutput
+ * Signature: (IIZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_setSolenoidOutput
+  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
+{
+  HALSIM_SetPCMSolenoidOutput(index, channel, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerCompressorInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerCompressorInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterPCMCompressorInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    cancelCompressorInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelCompressorInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPCMCompressorInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    getCompressorInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_getCompressorInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPCMCompressorInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    setCompressorInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_setCompressorInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPCMCompressorInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerCompressorOnCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerCompressorOnCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPCMCompressorOnCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    cancelCompressorOnCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelCompressorOnCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPCMCompressorOnCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    getCompressorOn
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_getCompressorOn
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPCMCompressorOn(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    setCompressorOn
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_setCompressorOn
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPCMCompressorOn(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerClosedLoopEnabledCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerClosedLoopEnabledCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPCMClosedLoopEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    cancelClosedLoopEnabledCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelClosedLoopEnabledCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPCMClosedLoopEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    getClosedLoopEnabled
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_getClosedLoopEnabled
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPCMClosedLoopEnabled(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    setClosedLoopEnabled
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_setClosedLoopEnabled
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPCMClosedLoopEnabled(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerPressureSwitchCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerPressureSwitchCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPCMPressureSwitchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    cancelPressureSwitchCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelPressureSwitchCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPCMPressureSwitchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    getPressureSwitch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_getPressureSwitch
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPCMPressureSwitch(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    setPressureSwitch
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_setPressureSwitch
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPCMPressureSwitch(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerCompressorCurrentCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerCompressorCurrentCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPCMCompressorCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    cancelCompressorCurrentCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelCompressorCurrentCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPCMCompressorCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    getCompressorCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_getCompressorCurrent
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPCMCompressorCurrent(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    setCompressorCurrent
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_setCompressorCurrent
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetPCMCompressorCurrent(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerAllNonSolenoidCallbacks
+ * Signature: (ILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerAllNonSolenoidCallbacks
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      [](int32_t index, HAL_NotifyCallback cb, void* param, HAL_Bool in) {
+        HALSIM_RegisterPCMAllNonSolenoidCallbacks(index, cb, param, in);
+        return 0;
+      });
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    registerAllSolenoidCallbacks
+ * Signature: (IILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerAllSolenoidCallbacks
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      [](int32_t index, int32_t channel, HAL_NotifyCallback cb, void* param,
+         HAL_Bool in) {
+        HALSIM_RegisterPCMAllSolenoidCallbacks(index, channel, cb, param, in);
+        return 0;
+      });
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PCMDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetPCMData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/PDPDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/PDPDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/PDPDataJNI.cpp
new file mode 100644
index 0000000..2ce6cbd
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/PDPDataJNI.cpp
@@ -0,0 +1,232 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_PDPDataJNI.h"
+#include "hal/simulation/PDPData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPDPInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPDPInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPDPInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPDPInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    registerTemperatureCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerTemperatureCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPDPTemperatureCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    cancelTemperatureCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelTemperatureCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPDPTemperatureCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    getTemperature
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_getTemperature
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPDPTemperature(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    setTemperature
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_setTemperature
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetPDPTemperature(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    registerVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerVoltageCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPDPVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    cancelVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelVoltageCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPDPVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_getVoltage
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPDPVoltage(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    setVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_setVoltage
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetPDPVoltage(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    registerCurrentCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerCurrentCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  return sim::AllocateChannelCallback(env, index, channel, callback,
+                                      initialNotify,
+                                      &HALSIM_RegisterPDPCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    cancelCurrentCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelCurrentCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+  return sim::FreeChannelCallback(env, handle, index, channel,
+                                  &HALSIM_CancelPDPCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    getCurrent
+ * Signature: (II)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_getCurrent
+  (JNIEnv*, jclass, jint index, jint channel)
+{
+  return HALSIM_GetPDPCurrent(index, channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    setCurrent
+ * Signature: (IID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_setCurrent
+  (JNIEnv*, jclass, jint index, jint channel, jdouble value)
+{
+  HALSIM_SetPDPCurrent(index, channel, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PDPDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetPDPData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp
new file mode 100644
index 0000000..03e61ef
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp
@@ -0,0 +1,329 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_PWMDataJNI.h"
+#include "hal/simulation/PWMData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPWMInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPWMInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPWMInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPWMInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    registerRawValueCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_registerRawValueCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPWMRawValueCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    cancelRawValueCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_cancelRawValueCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPWMRawValueCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    getRawValue
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_getRawValue
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPWMRawValue(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    setRawValue
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_setRawValue
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetPWMRawValue(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    registerSpeedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_registerSpeedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPWMSpeedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    cancelSpeedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_cancelSpeedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index, &HALSIM_CancelPWMSpeedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    getSpeed
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_getSpeed
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPWMSpeed(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    setSpeed
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_setSpeed
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetPWMSpeed(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    registerPositionCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_registerPositionCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPWMPositionCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    cancelPositionCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_cancelPositionCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPWMPositionCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    getPosition
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_getPosition
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPWMPosition(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    setPosition
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_setPosition
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetPWMPosition(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    registerPeriodScaleCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_registerPeriodScaleCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPWMPeriodScaleCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    cancelPeriodScaleCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_cancelPeriodScaleCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPWMPeriodScaleCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    getPeriodScale
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_getPeriodScale
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPWMPeriodScale(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    setPeriodScale
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_setPeriodScale
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetPWMPeriodScale(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    registerZeroLatchCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_registerZeroLatchCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterPWMZeroLatchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    cancelZeroLatchCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_cancelZeroLatchCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPWMZeroLatchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    getZeroLatch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_getZeroLatch
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPWMZeroLatch(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    setZeroLatch
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_setZeroLatch
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPWMZeroLatch(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PWMDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PWMDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetPWMData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp
new file mode 100644
index 0000000..7abe7cb
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp
@@ -0,0 +1,230 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_RelayDataJNI.h"
+#include "hal/simulation/RelayData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    registerInitializedForwardCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_registerInitializedForwardCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterRelayInitializedForwardCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    cancelInitializedForwardCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_cancelInitializedForwardCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelRelayInitializedForwardCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    getInitializedForward
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_getInitializedForward
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetRelayInitializedForward(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    setInitializedForward
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_setInitializedForward
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetRelayInitializedForward(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    registerInitializedReverseCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_registerInitializedReverseCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterRelayInitializedReverseCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    cancelInitializedReverseCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_cancelInitializedReverseCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelRelayInitializedReverseCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    getInitializedReverse
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_getInitializedReverse
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetRelayInitializedReverse(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    setInitializedReverse
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_setInitializedReverse
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetRelayInitializedReverse(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    registerForwardCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_registerForwardCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterRelayForwardCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    cancelForwardCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_cancelForwardCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelRelayForwardCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    getForward
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_getForward
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetRelayForward(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    setForward
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_setForward
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetRelayForward(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    registerReverseCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_registerReverseCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterRelayReverseCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    cancelReverseCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_cancelReverseCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelRelayReverseCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    getReverse
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_getReverse
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetRelayReverse(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    setReverse
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_setReverse
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetRelayReverse(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RelayDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RelayDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetRelayData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
new file mode 100644
index 0000000..1ff6044
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
@@ -0,0 +1,792 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_RoboRioDataJNI.h"
+#include "hal/simulation/RoboRioData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerFPGAButtonCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerFPGAButtonCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify, &HALSIM_RegisterRoboRioFPGAButtonCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelFPGAButtonCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelFPGAButtonCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioFPGAButtonCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getFPGAButton
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getFPGAButton
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioFPGAButton();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setFPGAButton
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setFPGAButton
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetRoboRioFPGAButton(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerVInVoltageCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerVInVoltageCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify, &HALSIM_RegisterRoboRioVInVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelVInVoltageCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelVInVoltageCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioVInVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getVInVoltage
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getVInVoltage
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioVInVoltage();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setVInVoltage
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setVInVoltage
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioVInVoltage(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerVInCurrentCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerVInCurrentCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify, &HALSIM_RegisterRoboRioVInCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelVInCurrentCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelVInCurrentCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioVInCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getVInCurrent
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getVInCurrent
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioVInCurrent();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setVInCurrent
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setVInCurrent
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioVInCurrent(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserVoltage6VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserVoltage6VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserVoltage6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserVoltage6VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserVoltage6VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserVoltage6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserVoltage6V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserVoltage6V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserVoltage6V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserVoltage6V
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserVoltage6V
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioUserVoltage6V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserCurrent6VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserCurrent6VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserCurrent6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserCurrent6VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserCurrent6VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserCurrent6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserCurrent6V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserCurrent6V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserCurrent6V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserCurrent6V
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserCurrent6V
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioUserCurrent6V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserActive6VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserActive6VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserActive6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserActive6VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserActive6VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserActive6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserActive6V
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserActive6V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserActive6V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserActive6V
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserActive6V
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetRoboRioUserActive6V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserVoltage5VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserVoltage5VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserVoltage5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserVoltage5VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserVoltage5VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserVoltage5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserVoltage5V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserVoltage5V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserVoltage5V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserVoltage5V
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserVoltage5V
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioUserVoltage5V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserCurrent5VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserCurrent5VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserCurrent5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserCurrent5VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserCurrent5VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserCurrent5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserCurrent5V
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserCurrent5V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserCurrent5V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserCurrent5V
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserCurrent5V
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioUserCurrent5V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserActive5VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserActive5VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserActive5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserActive5VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserActive5VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserActive5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserActive5V
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserActive5V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserActive5V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserActive5V
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserActive5V
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetRoboRioUserActive5V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserVoltage3V3Callback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserVoltage3V3Callback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserVoltage3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserVoltage3V3Callback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserVoltage3V3Callback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserVoltage3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserVoltage3V3
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserVoltage3V3
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserVoltage3V3();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserVoltage3V3
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserVoltage3V3
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioUserVoltage3V3(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserCurrent3V3Callback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserCurrent3V3Callback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserCurrent3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserCurrent3V3Callback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserCurrent3V3Callback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserCurrent3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserCurrent3V3
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserCurrent3V3
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserCurrent3V3();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserCurrent3V3
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserCurrent3V3
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioUserCurrent3V3(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserActive3V3Callback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserActive3V3Callback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserActive3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserActive3V3Callback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserActive3V3Callback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserActive3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserActive3V3
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserActive3V3
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserActive3V3();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserActive3V3
+ * Signature: (Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserActive3V3
+  (JNIEnv*, jclass, jboolean value)
+{
+  HALSIM_SetRoboRioUserActive3V3(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserFaults6VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserFaults6VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserFaults6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserFaults6VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserFaults6VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserFaults6VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserFaults6V
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserFaults6V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserFaults6V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserFaults6V
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserFaults6V
+  (JNIEnv*, jclass, jint value)
+{
+  HALSIM_SetRoboRioUserFaults6V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserFaults5VCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserFaults5VCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserFaults5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserFaults5VCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserFaults5VCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserFaults5VCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserFaults5V
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserFaults5V
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserFaults5V();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserFaults5V
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserFaults5V
+  (JNIEnv*, jclass, jint value)
+{
+  HALSIM_SetRoboRioUserFaults5V(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerUserFaults3V3Callback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerUserFaults3V3Callback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioUserFaults3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelUserFaults3V3Callback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelUserFaults3V3Callback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioUserFaults3V3Callback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getUserFaults3V3
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getUserFaults3V3
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioUserFaults3V3();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setUserFaults3V3
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setUserFaults3V3
+  (JNIEnv*, jclass, jint value)
+{
+  HALSIM_SetRoboRioUserFaults3V3(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    resetData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_resetData
+  (JNIEnv*, jclass)
+{
+  HALSIM_ResetRoboRioData();
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp
new file mode 100644
index 0000000..fe0c410
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp
@@ -0,0 +1,280 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI.h"
+#include "hal/simulation/SPIAccelerometerData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    registerActiveCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerActiveCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterSPIAccelerometerActiveCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    cancelActiveCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelActiveCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelSPIAccelerometerActiveCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    getActive
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getActive
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetSPIAccelerometerActive(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    setActive
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setActive
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetSPIAccelerometerActive(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    registerRangeCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerRangeCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterSPIAccelerometerRangeCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    cancelRangeCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelRangeCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelSPIAccelerometerRangeCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    getRange
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getRange
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetSPIAccelerometerRange(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    setRange
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setRange
+  (JNIEnv*, jclass, jint index, jint value)
+{
+  HALSIM_SetSPIAccelerometerRange(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    registerXCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerXCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterSPIAccelerometerXCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    cancelXCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelXCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelSPIAccelerometerXCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    getX
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getX
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetSPIAccelerometerX(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    setX
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setX
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetSPIAccelerometerX(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    registerYCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerYCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterSPIAccelerometerYCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    cancelYCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelYCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelSPIAccelerometerYCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    getY
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getY
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetSPIAccelerometerY(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    setY
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setY
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetSPIAccelerometerY(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    registerZCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerZCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterSPIAccelerometerZCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    cancelZCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelZCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelSPIAccelerometerZCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    getZ
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getZ
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetSPIAccelerometerZ(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    setZ
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setZ
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetSPIAccelerometerZ(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetSPIAccelerometerData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp
new file mode 100644
index 0000000..3709984
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp
@@ -0,0 +1,160 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include "BufferCallbackStore.h"
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "SpiReadAutoReceiveBufferCallbackStore.h"
+#include "edu_wpi_first_hal_simulation_SPIDataJNI.h"
+#include "hal/simulation/SPIData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterSPIInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelSPIInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetSPIInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetSPIInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    registerReadCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerReadCallback
+  (JNIEnv* env, jclass, jint index, jobject callback)
+{
+  return sim::AllocateBufferCallback(env, index, callback,
+                                     &HALSIM_RegisterSPIReadCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    cancelReadCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelReadCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  sim::FreeBufferCallback(env, handle, index, &HALSIM_CancelSPIReadCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    registerWriteCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerWriteCallback
+  (JNIEnv* env, jclass, jint index, jobject callback)
+{
+  return sim::AllocateConstBufferCallback(env, index, callback,
+                                          &HALSIM_RegisterSPIWriteCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    cancelWriteCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelWriteCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  sim::FreeConstBufferCallback(env, handle, index,
+                               &HALSIM_CancelSPIWriteCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    registerReadAutoReceiveBufferCallback
+ * Signature: (ILjava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerReadAutoReceiveBufferCallback
+  (JNIEnv* env, jclass, jint index, jobject callback)
+{
+  return sim::AllocateSpiBufferCallback(
+      env, index, callback, &HALSIM_RegisterSPIReadAutoReceivedDataCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    cancelReadAutoReceiveBufferCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelReadAutoReceiveBufferCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  sim::FreeSpiBufferCallback(env, handle, index,
+                             &HALSIM_CancelSPIReadAutoReceivedDataCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SPIDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SPIDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetSPIData(index);
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp
new file mode 100644
index 0000000..0b43d24
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp
@@ -0,0 +1,602 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "SimDeviceDataJNI.h"
+
+#include <jni.h>
+
+#include <functional>
+#include <string>
+#include <utility>
+
+#include <wpi/UidVector.h>
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "edu_wpi_first_hal_simulation_SimDeviceDataJNI.h"
+#include "hal/simulation/SimDeviceData.h"
+
+using namespace hal;
+using namespace wpi::java;
+
+static JClass simDeviceInfoCls;
+static JClass simValueInfoCls;
+static JClass simDeviceCallbackCls;
+static JClass simValueCallbackCls;
+static jmethodID simDeviceCallbackCallback;
+static jmethodID simValueCallbackCallback;
+
+namespace {
+
+struct DeviceInfo {
+  DeviceInfo(const char* name_, HAL_SimDeviceHandle handle_)
+      : name{name_}, handle{handle_} {}
+  std::string name;
+  HAL_SimValueHandle handle;
+
+  jobject MakeJava(JNIEnv* env) const;
+  void CallJava(JNIEnv* env, jobject callobj) const;
+};
+
+struct ValueInfo {
+  ValueInfo(const char* name_, HAL_SimValueHandle handle_, bool readonly_,
+            const HAL_Value& value_)
+      : name{name_}, handle{handle_}, readonly{readonly_}, value{value_} {}
+  std::string name;
+  HAL_SimValueHandle handle;
+  bool readonly;
+  HAL_Value value;
+
+  jobject MakeJava(JNIEnv* env) const;
+  void CallJava(JNIEnv* env, jobject callobj) const;
+
+ private:
+  std::pair<jlong, jdouble> ToValue12() const;
+};
+
+}  // namespace
+
+jobject DeviceInfo::MakeJava(JNIEnv* env) const {
+  static jmethodID func =
+      env->GetMethodID(simDeviceInfoCls, "<init>", "(Ljava/lang/String;I)V");
+  return env->NewObject(simDeviceInfoCls, func, MakeJString(env, name),
+                        (jint)handle);
+}
+
+void DeviceInfo::CallJava(JNIEnv* env, jobject callobj) const {
+  env->CallVoidMethod(callobj, simDeviceCallbackCallback,
+                      MakeJString(env, name), (jint)handle);
+}
+
+std::pair<jlong, jdouble> ValueInfo::ToValue12() const {
+  jlong value1 = 0;
+  jdouble value2 = 0.0;
+  switch (value.type) {
+    case HAL_BOOLEAN:
+      value1 = value.data.v_boolean;
+      break;
+    case HAL_DOUBLE:
+      value2 = value.data.v_double;
+      break;
+    case HAL_ENUM:
+      value1 = value.data.v_enum;
+      break;
+    case HAL_INT:
+      value1 = value.data.v_int;
+      break;
+    case HAL_LONG:
+      value1 = value.data.v_long;
+      break;
+    default:
+      break;
+  }
+  return std::pair(value1, value2);
+}
+
+jobject ValueInfo::MakeJava(JNIEnv* env) const {
+  static jmethodID func =
+      env->GetMethodID(simValueInfoCls, "<init>", "(Ljava/lang/String;IZIJD)V");
+  auto [value1, value2] = ToValue12();
+  return env->NewObject(simValueInfoCls, func, MakeJString(env, name),
+                        (jint)handle, (jboolean)readonly, (jint)value.type,
+                        value1, value2);
+}
+
+void ValueInfo::CallJava(JNIEnv* env, jobject callobj) const {
+  auto [value1, value2] = ToValue12();
+  env->CallVoidMethod(callobj, simValueCallbackCallback, MakeJString(env, name),
+                      (jint)handle, (jboolean)readonly, (jint)value.type,
+                      value1, value2);
+}
+
+namespace {
+
+class CallbackStore {
+ public:
+  explicit CallbackStore(JNIEnv* env, jobject obj) : m_call{env, obj} {}
+  ~CallbackStore() {
+    if (m_cancelCallback) m_cancelCallback();
+  }
+
+  void SetCancel(std::function<void()> cancelCallback) {
+    m_cancelCallback = std::move(cancelCallback);
+  }
+  void Free(JNIEnv* env) { m_call.free(env); }
+  jobject Get() const { return m_call; }
+
+ private:
+  wpi::java::JGlobal<jobject> m_call;
+  std::function<void()> m_cancelCallback;
+};
+
+class CallbackThreadJNI : public wpi::SafeThread {
+ public:
+  void Main();
+
+  using DeviceCalls =
+      std::vector<std::pair<std::weak_ptr<CallbackStore>, DeviceInfo>>;
+  DeviceCalls m_deviceCalls;
+  using ValueCalls =
+      std::vector<std::pair<std::weak_ptr<CallbackStore>, ValueInfo>>;
+  ValueCalls m_valueCalls;
+
+  wpi::UidVector<std::shared_ptr<CallbackStore>, 4> m_callbacks;
+};
+
+class CallbackJNI {
+ public:
+  static CallbackJNI& GetInstance() {
+    static CallbackJNI inst;
+    return inst;
+  }
+  void SendDevice(int32_t callback, DeviceInfo info);
+  void SendValue(int32_t callback, ValueInfo info);
+
+  std::pair<int32_t, std::shared_ptr<CallbackStore>> AllocateCallback(
+      JNIEnv* env, jobject obj);
+
+  void FreeCallback(JNIEnv* env, int32_t uid);
+
+ private:
+  CallbackJNI() { m_owner.Start(); }
+
+  wpi::SafeThreadOwner<CallbackThreadJNI> m_owner;
+};
+
+}  // namespace
+
+void CallbackThreadJNI::Main() {
+  JNIEnv* env;
+  JavaVMAttachArgs args;
+  args.version = JNI_VERSION_1_2;
+  args.name = const_cast<char*>("SimDeviceCallback");
+  args.group = nullptr;
+  jint rs = sim::GetJVM()->AttachCurrentThreadAsDaemon(
+      reinterpret_cast<void**>(&env), &args);
+  if (rs != JNI_OK) return;
+
+  DeviceCalls deviceCalls;
+  ValueCalls valueCalls;
+
+  std::unique_lock lock(m_mutex);
+  while (m_active) {
+    m_cond.wait(lock, [&] { return !m_active; });
+    if (!m_active) break;
+
+    deviceCalls.swap(m_deviceCalls);
+    valueCalls.swap(m_valueCalls);
+
+    lock.unlock();  // don't hold mutex during callback execution
+
+    for (auto&& call : deviceCalls) {
+      if (auto store = call.first.lock()) {
+        if (jobject callobj = store->Get()) {
+          call.second.CallJava(env, callobj);
+          if (env->ExceptionCheck()) {
+            env->ExceptionDescribe();
+            env->ExceptionClear();
+          }
+        }
+      }
+    }
+
+    for (auto&& call : valueCalls) {
+      if (auto store = call.first.lock()) {
+        if (jobject callobj = store->Get()) {
+          call.second.CallJava(env, callobj);
+          if (env->ExceptionCheck()) {
+            env->ExceptionDescribe();
+            env->ExceptionClear();
+          }
+        }
+      }
+    }
+
+    deviceCalls.clear();
+    valueCalls.clear();
+
+    lock.lock();
+  }
+
+  // free global references
+  for (auto&& callback : m_callbacks) callback->Free(env);
+
+  sim::GetJVM()->DetachCurrentThread();
+}
+
+void CallbackJNI::SendDevice(int32_t callback, DeviceInfo info) {
+  auto thr = m_owner.GetThread();
+  if (!thr) return;
+  thr->m_deviceCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
+  thr->m_cond.notify_one();
+}
+
+void CallbackJNI::SendValue(int32_t callback, ValueInfo info) {
+  auto thr = m_owner.GetThread();
+  if (!thr) return;
+  thr->m_valueCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
+  thr->m_cond.notify_one();
+}
+
+std::pair<int32_t, std::shared_ptr<CallbackStore>>
+CallbackJNI::AllocateCallback(JNIEnv* env, jobject obj) {
+  auto thr = m_owner.GetThread();
+  if (!thr) return std::pair(0, nullptr);
+  auto store = std::make_shared<CallbackStore>(env, obj);
+  return std::pair(thr->m_callbacks.emplace_back(store) + 1, store);
+}
+
+void CallbackJNI::FreeCallback(JNIEnv* env, int32_t uid) {
+  auto thr = m_owner.GetThread();
+  if (!thr) return;
+  if (uid <= 0 || static_cast<uint32_t>(uid) >= thr->m_callbacks.size()) return;
+  --uid;
+  auto store = std::move(thr->m_callbacks[uid]);
+  thr->m_callbacks.erase(uid);
+  store->Free(env);
+}
+
+namespace hal {
+namespace sim {
+
+bool InitializeSimDeviceDataJNI(JNIEnv* env) {
+  simDeviceInfoCls = JClass(
+      env, "edu/wpi/first/hal/simulation/SimDeviceDataJNI$SimDeviceInfo");
+  if (!simDeviceInfoCls) return false;
+
+  simValueInfoCls =
+      JClass(env, "edu/wpi/first/hal/simulation/SimDeviceDataJNI$SimValueInfo");
+  if (!simValueInfoCls) return false;
+
+  simDeviceCallbackCls =
+      JClass(env, "edu/wpi/first/hal/simulation/SimDeviceCallback");
+  if (!simDeviceCallbackCls) return false;
+
+  simDeviceCallbackCallback = env->GetMethodID(simDeviceCallbackCls, "callback",
+                                               "(Ljava/lang/String;I)V");
+  if (!simDeviceCallbackCallback) return false;
+
+  simValueCallbackCls =
+      JClass(env, "edu/wpi/first/hal/simulation/SimValueCallback");
+  if (!simValueCallbackCls) return false;
+
+  simValueCallbackCallback = env->GetMethodID(
+      simValueCallbackCls, "callbackNative", "(Ljava/lang/String;IZIJD)V");
+  if (!simValueCallbackCallback) return false;
+
+  return true;
+}
+
+void FreeSimDeviceDataJNI(JNIEnv* env) {
+  simDeviceInfoCls.free(env);
+  simValueInfoCls.free(env);
+  simDeviceCallbackCls.free(env);
+  simValueCallbackCls.free(env);
+}
+
+}  // namespace sim
+}  // namespace hal
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    setSimDeviceEnabled
+ * Signature: (Ljava/lang/String;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_setSimDeviceEnabled
+  (JNIEnv* env, jclass, jstring prefix, jboolean enabled)
+{
+  HALSIM_SetSimDeviceEnabled(JStringRef{env, prefix}.c_str(), enabled);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    isSimDeviceEnabled
+ * Signature: (Ljava/lang/String;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_isSimDeviceEnabled
+  (JNIEnv* env, jclass, jstring name)
+{
+  return HALSIM_IsSimDeviceEnabled(JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    registerSimDeviceCreatedCallback
+ * Signature: (Ljava/lang/String;Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimDeviceCreatedCallback
+  (JNIEnv* env, jclass, jstring prefix, jobject callback,
+   jboolean initialNotify)
+{
+  auto [uid, store] =
+      CallbackJNI::GetInstance().AllocateCallback(env, callback);
+  int32_t cuid = HALSIM_RegisterSimDeviceCreatedCallback(
+      JStringRef{env, prefix}.c_str(),
+      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+        int32_t uid = reinterpret_cast<intptr_t>(param);
+        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
+      },
+      initialNotify);
+  store->SetCancel([cuid] { HALSIM_CancelSimDeviceCreatedCallback(cuid); });
+  return uid;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    cancelSimDeviceCreatedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimDeviceCreatedCallback
+  (JNIEnv* env, jclass, jint uid)
+{
+  CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    registerSimDeviceFreedCallback
+ * Signature: (Ljava/lang/String;Ljava/lang/Object;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimDeviceFreedCallback
+  (JNIEnv* env, jclass, jstring prefix, jobject callback)
+{
+  auto [uid, store] =
+      CallbackJNI::GetInstance().AllocateCallback(env, callback);
+  int32_t cuid = HALSIM_RegisterSimDeviceFreedCallback(
+      JStringRef{env, prefix}.c_str(),
+      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+        int32_t uid = reinterpret_cast<intptr_t>(param);
+        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
+      });
+  store->SetCancel([cuid] { HALSIM_CancelSimDeviceFreedCallback(cuid); });
+  return uid;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    cancelSimDeviceFreedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimDeviceFreedCallback
+  (JNIEnv* env, jclass, jint uid)
+{
+  CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    getSimDeviceHandle
+ * Signature: (Ljava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_getSimDeviceHandle
+  (JNIEnv* env, jclass, jstring name)
+{
+  return HALSIM_GetSimDeviceHandle(JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    getSimValueDeviceHandle
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_getSimValueDeviceHandle
+  (JNIEnv*, jclass, jint handle)
+{
+  return HALSIM_GetSimValueDeviceHandle(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    enumerateSimDevices
+ * Signature: (Ljava/lang/String;)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_enumerateSimDevices
+  (JNIEnv* env, jclass, jstring prefix)
+{
+  // get values
+  std::vector<DeviceInfo> arr;
+  HALSIM_EnumerateSimDevices(
+      JStringRef{env, prefix}.c_str(), &arr,
+      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+        auto arr = static_cast<std::vector<DeviceInfo>*>(param);
+        arr->emplace_back(name, handle);
+      });
+
+  // convert to java
+  size_t numElems = arr.size();
+  jobjectArray jarr =
+      env->NewObjectArray(arr.size(), simDeviceInfoCls, nullptr);
+  if (!jarr) return nullptr;
+  for (size_t i = 0; i < numElems; ++i) {
+    JLocal<jobject> elem{env, arr[i].MakeJava(env)};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    registerSimValueCreatedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimValueCreatedCallback
+  (JNIEnv* env, jclass, jint device, jobject callback, jboolean initialNotify)
+{
+  auto [uid, store] =
+      CallbackJNI::GetInstance().AllocateCallback(env, callback);
+  int32_t cuid = HALSIM_RegisterSimValueCreatedCallback(
+      device, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+      [](const char* name, void* param, HAL_SimValueHandle handle,
+         HAL_Bool readonly, const HAL_Value* value) {
+        int32_t uid = reinterpret_cast<intptr_t>(param);
+        CallbackJNI::GetInstance().SendValue(
+            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
+      },
+      initialNotify);
+  store->SetCancel([cuid] { HALSIM_CancelSimValueCreatedCallback(cuid); });
+  return uid;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    cancelSimValueCreatedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimValueCreatedCallback
+  (JNIEnv* env, jclass, jint uid)
+{
+  CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    registerSimValueChangedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimValueChangedCallback
+  (JNIEnv* env, jclass, jint handle, jobject callback, jboolean initialNotify)
+{
+  auto [uid, store] =
+      CallbackJNI::GetInstance().AllocateCallback(env, callback);
+  int32_t cuid = HALSIM_RegisterSimValueChangedCallback(
+      handle, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
+      [](const char* name, void* param, HAL_SimValueHandle handle,
+         HAL_Bool readonly, const HAL_Value* value) {
+        int32_t uid = reinterpret_cast<intptr_t>(param);
+        CallbackJNI::GetInstance().SendValue(
+            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
+      },
+      initialNotify);
+  store->SetCancel([cuid] { HALSIM_CancelSimValueChangedCallback(cuid); });
+  return uid;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    cancelSimValueChangedCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimValueChangedCallback
+  (JNIEnv* env, jclass, jint uid)
+{
+  CallbackJNI::GetInstance().FreeCallback(env, uid);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    getSimValueHandle
+ * Signature: (ILjava/lang/String;)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_getSimValueHandle
+  (JNIEnv* env, jclass, jint device, jstring name)
+{
+  return HALSIM_GetSimValueHandle(device, JStringRef{env, name}.c_str());
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    enumerateSimValues
+ * Signature: (I)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_enumerateSimValues
+  (JNIEnv* env, jclass, jint device)
+{
+  // get values
+  std::vector<ValueInfo> arr;
+  HALSIM_EnumerateSimValues(
+      device, &arr,
+      [](const char* name, void* param, HAL_SimValueHandle handle,
+         HAL_Bool readonly, const HAL_Value* value) {
+        auto arr = static_cast<std::vector<ValueInfo>*>(param);
+        arr->emplace_back(name, handle, readonly, *value);
+      });
+
+  // convert to java
+  size_t numElems = arr.size();
+  jobjectArray jarr = env->NewObjectArray(arr.size(), simValueInfoCls, nullptr);
+  if (!jarr) return nullptr;
+  for (size_t i = 0; i < numElems; ++i) {
+    JLocal<jobject> elem{env, arr[i].MakeJava(env)};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    getSimValueEnumOptions
+ * Signature: (I)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_getSimValueEnumOptions
+  (JNIEnv* env, jclass, jint handle)
+{
+  static JClass stringCls{env, "java/lang/String"};
+  if (!stringCls) return nullptr;
+  int32_t numElems = 0;
+  const char** elems = HALSIM_GetSimValueEnumOptions(handle, &numElems);
+  jobjectArray jarr = env->NewObjectArray(numElems, stringCls, nullptr);
+  if (!jarr) return nullptr;
+  for (int32_t i = 0; i < numElems; ++i) {
+    JLocal<jstring> elem{env, MakeJString(env, elems[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    resetSimDeviceData
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_resetSimDeviceData
+  (JNIEnv*, jclass)
+{
+  HALSIM_ResetSimDeviceData();
+}
+
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h

diff --git a/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h b/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h
new file mode 100644
index 0000000..44fc27f
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <jni.h>
+
+namespace hal {
+namespace sim {
+bool InitializeSimDeviceDataJNI(JNIEnv* env);
+void FreeSimDeviceDataJNI(JNIEnv* env);
+}  // namespace sim
+}  // namespace hal

hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
new file mode 100644
index 0000000..b6336e8
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
@@ -0,0 +1,244 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "SimulatorJNI.h"
+
+#include <wpi/jni_util.h>
+
+#include "BufferCallbackStore.h"
+#include "CallbackStore.h"
+#include "ConstBufferCallbackStore.h"
+#include "SimDeviceDataJNI.h"
+#include "SpiReadAutoReceiveBufferCallbackStore.h"
+#include "edu_wpi_first_hal_simulation_SimulatorJNI.h"
+#include "hal/HAL.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/MockHooks.h"
+
+using namespace wpi::java;
+
+static JavaVM* jvm = nullptr;
+static JClass notifyCallbackCls;
+static JClass bufferCallbackCls;
+static JClass constBufferCallbackCls;
+static JClass spiReadAutoReceiveBufferCallbackCls;
+static jmethodID notifyCallbackCallback;
+static jmethodID bufferCallbackCallback;
+static jmethodID constBufferCallbackCallback;
+static jmethodID spiReadAutoReceiveBufferCallbackCallback;
+
+namespace hal {
+namespace sim {
+jint SimOnLoad(JavaVM* vm, void* reserved) {
+  jvm = vm;
+
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+    return JNI_ERR;
+
+  notifyCallbackCls =
+      JClass(env, "edu/wpi/first/hal/simulation/NotifyCallback");
+  if (!notifyCallbackCls) return JNI_ERR;
+
+  notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative",
+                                            "(Ljava/lang/String;IJD)V");
+  if (!notifyCallbackCallback) return JNI_ERR;
+
+  bufferCallbackCls =
+      JClass(env, "edu/wpi/first/hal/simulation/BufferCallback");
+  if (!bufferCallbackCls) return JNI_ERR;
+
+  bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback",
+                                            "(Ljava/lang/String;[BI)V");
+  if (!bufferCallbackCallback) return JNI_ERR;
+
+  constBufferCallbackCls =
+      JClass(env, "edu/wpi/first/hal/simulation/ConstBufferCallback");
+  if (!constBufferCallbackCls) return JNI_ERR;
+
+  constBufferCallbackCallback = env->GetMethodID(
+      constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V");
+  if (!constBufferCallbackCallback) return JNI_ERR;
+
+  spiReadAutoReceiveBufferCallbackCls = JClass(
+      env, "edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback");
+  if (!spiReadAutoReceiveBufferCallbackCls) return JNI_ERR;
+
+  spiReadAutoReceiveBufferCallbackCallback =
+      env->GetMethodID(spiReadAutoReceiveBufferCallbackCls, "callback",
+                       "(Ljava/lang/String;[II)I");
+  if (!spiReadAutoReceiveBufferCallbackCallback) return JNI_ERR;
+
+  InitializeStore();
+  InitializeBufferStore();
+  InitializeConstBufferStore();
+  InitializeSpiBufferStore();
+  if (!InitializeSimDeviceDataJNI(env)) return JNI_ERR;
+
+  return JNI_VERSION_1_6;
+}
+
+void SimOnUnload(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+    return;
+
+  notifyCallbackCls.free(env);
+  bufferCallbackCls.free(env);
+  constBufferCallbackCls.free(env);
+  spiReadAutoReceiveBufferCallbackCls.free(env);
+  FreeSimDeviceDataJNI(env);
+  jvm = nullptr;
+}
+
+JavaVM* GetJVM() { return jvm; }
+
+jmethodID GetNotifyCallback() { return notifyCallbackCallback; }
+
+jmethodID GetBufferCallback() { return bufferCallbackCallback; }
+
+jmethodID GetConstBufferCallback() { return constBufferCallbackCallback; }
+
+jmethodID GetSpiReadAutoReceiveBufferCallback() {
+  return spiReadAutoReceiveBufferCallbackCallback;
+}
+}  // namespace sim
+}  // namespace hal
+
+extern "C" {
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    setRuntimeType
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_setRuntimeType
+  (JNIEnv*, jclass, jint type)
+{
+  HALSIM_SetRuntimeType(static_cast<HAL_RuntimeType>(type));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    waitForProgramStart
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_waitForProgramStart
+  (JNIEnv*, jclass)
+{
+  HALSIM_WaitForProgramStart();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    setProgramStarted
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_setProgramStarted
+  (JNIEnv*, jclass)
+{
+  HALSIM_SetProgramStarted();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    getProgramStarted
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_getProgramStarted
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetProgramStarted();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    restartTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_restartTiming
+  (JNIEnv*, jclass)
+{
+  HALSIM_RestartTiming();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    pauseTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_pauseTiming
+  (JNIEnv*, jclass)
+{
+  HALSIM_PauseTiming();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    resumeTiming
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_resumeTiming
+  (JNIEnv*, jclass)
+{
+  HALSIM_ResumeTiming();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    isTimingPaused
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_isTimingPaused
+  (JNIEnv*, jclass)
+{
+  return HALSIM_IsTimingPaused();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    stepTiming
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_stepTiming
+  (JNIEnv*, jclass, jlong delta)
+{
+  HALSIM_StepTiming(delta);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    stepTimingAsync
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_stepTimingAsync
+  (JNIEnv*, jclass, jlong delta)
+{
+  HALSIM_StepTimingAsync(delta);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimulatorJNI
+ * Method:    resetHandles
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimulatorJNI_resetHandles
+  (JNIEnv*, jclass)
+{
+  hal::HandleBase::ResetGlobalHandles();
+}
+}  // extern "C"

hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h

diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
new file mode 100644
index 0000000..d6710e8
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "jni.h"
+
+typedef HAL_Handle SIM_JniHandle;
+
+namespace hal {
+namespace sim {
+JavaVM* GetJVM();
+
+jmethodID GetNotifyCallback();
+jmethodID GetBufferCallback();
+jmethodID GetConstBufferCallback();
+jmethodID GetSpiReadAutoReceiveBufferCallback();
+}  // namespace sim
+}  // namespace hal

hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp

diff --git a/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp b/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp
new file mode 100644
index 0000000..5432840
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "SpiReadAutoReceiveBufferCallbackStore.h"
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+
+using namespace hal;
+using namespace hal::sim;
+using namespace wpi::java;
+
+static hal::UnlimitedHandleResource<
+    SIM_JniHandle, SpiReadAutoReceiveBufferCallbackStore,
+    hal::HAL_HandleEnum::SimulationJni>* callbackHandles;
+
+namespace hal {
+namespace sim {
+void InitializeSpiBufferStore() {
+  static hal::UnlimitedHandleResource<SIM_JniHandle,
+                                      SpiReadAutoReceiveBufferCallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cb;
+  callbackHandles = &cb;
+}
+}  // namespace sim
+}  // namespace hal
+
+void SpiReadAutoReceiveBufferCallbackStore::create(JNIEnv* env, jobject obj) {
+  m_call = JGlobal<jobject>(env, obj);
+}
+
+int32_t SpiReadAutoReceiveBufferCallbackStore::performCallback(
+    const char* name, uint32_t* buffer, int32_t numToRead) {
+  JNIEnv* env;
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      wpi::outs() << "Failed to attach\n";
+      wpi::outs().flush();
+      return -1;
+    }
+  } else if (tryGetEnv == JNI_EVERSION) {
+    wpi::outs() << "Invalid JVM Version requested\n";
+    wpi::outs().flush();
+  }
+
+  auto toCallbackArr = MakeJIntArray(
+      env, wpi::ArrayRef<uint32_t>{buffer, static_cast<size_t>(numToRead)});
+
+  jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(),
+                                MakeJString(env, name), toCallbackArr,
+                                (jint)numToRead);
+
+  jint* fromCallbackArr = reinterpret_cast<jint*>(
+      env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
+
+  for (int i = 0; i < ret; i++) {
+    buffer[i] = fromCallbackArr[i];
+  }
+
+  env->ReleasePrimitiveArrayCritical(toCallbackArr, fromCallbackArr, JNI_ABORT);
+
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
+  return ret;
+}
+
+void SpiReadAutoReceiveBufferCallbackStore::free(JNIEnv* env) {
+  m_call.free(env);
+}
+
+SIM_JniHandle sim::AllocateSpiBufferCallback(
+    JNIEnv* env, jint index, jobject callback,
+    RegisterSpiBufferCallbackFunc createCallback) {
+  auto callbackStore =
+      std::make_shared<SpiReadAutoReceiveBufferCallbackStore>();
+
+  auto handle = callbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param, uint32_t* buffer,
+                         int32_t numToRead, int32_t* outputCount) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = callbackHandles->Get(handle);
+    if (!data) return;
+
+    *outputCount = data->performCallback(name, buffer, numToRead);
+  };
+
+  auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
+}
+
+void sim::FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                                FreeSpiBufferCallbackFunc freeCallback) {
+  auto callback = callbackHandles->Free(handle);
+  freeCallback(index, callback->getCallbackId());
+  callback->free(env);
+}

hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h

diff --git a/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h b/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h
new file mode 100644
index 0000000..6a457de
--- /dev/null
+++ b/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "SimulatorJNI.h"
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/handles/UnlimitedHandleResource.h"
+#include "hal/simulation/NotifyListener.h"
+#include "hal/simulation/SPIData.h"
+
+namespace hal {
+namespace sim {
+class SpiReadAutoReceiveBufferCallbackStore {
+ public:
+  void create(JNIEnv* env, jobject obj);
+  int32_t performCallback(const char* name, uint32_t* buffer,
+                          int32_t numToRead);
+  void free(JNIEnv* env);
+  void setCallbackId(int32_t id) { callbackId = id; }
+  int32_t getCallbackId() { return callbackId; }
+
+ private:
+  wpi::java::JGlobal<jobject> m_call;
+  int32_t callbackId;
+};
+
+void InitializeSpiBufferStore();
+
+typedef int32_t (*RegisterSpiBufferCallbackFunc)(
+    int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
+typedef void (*FreeSpiBufferCallbackFunc)(int32_t index, int32_t uid);
+
+SIM_JniHandle AllocateSpiBufferCallback(
+    JNIEnv* env, jint index, jobject callback,
+    RegisterSpiBufferCallbackFunc createCallback);
+void FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
+                           FreeSpiBufferCallbackFunc freeCallback);
+}  // namespace sim
+}  // namespace hal

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

diff --git a/hal/src/main/native/include/hal/CAN.h b/hal/src/main/native/include/hal/CAN.h
index 8cc9c17..4f91bc1 100644
--- a/hal/src/main/native/include/hal/CAN.h
+++ b/hal/src/main/native/include/hal/CAN.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,8 +9,6 @@
 
 #include <stdint.h>
 
-#include "hal/Types.h"
-
 /**
  * @defgroup hal_canstream CAN Stream Functions
  * @ingroup hal_capi

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

diff --git a/hal/src/main/native/include/hal/CANAPITypes.h b/hal/src/main/native/include/hal/CANAPITypes.h
index a23cee1..5155d93 100644
--- a/hal/src/main/native/include/hal/CANAPITypes.h
+++ b/hal/src/main/native/include/hal/CANAPITypes.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -35,6 +35,7 @@
   HAL_CAN_Dev_kPowerDistribution = 8,
   HAL_CAN_Dev_kPneumatics = 9,
   HAL_CAN_Dev_kMiscellaneous = 10,
+  HAL_CAN_Dev_kIOBreakout = 11,
   HAL_CAN_Dev_kFirmwareUpdate = 31
 };
 

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

diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h
index 471c18b..23595db 100644
--- a/hal/src/main/native/include/hal/DriverStation.h
+++ b/hal/src/main/native/include/hal/DriverStation.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2013-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2013-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -37,6 +37,12 @@
 int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
                       const char* details, const char* location,
                       const char* callStack, HAL_Bool printMsg);
+/**
+ * Sends a line to the driver station console.
+ *
+ * @param line the line to send (null terminated)
+ */
+int32_t HAL_SendConsoleLine(const char* line);
 
 /**
  * Gets the current control word of the driver station.
@@ -171,7 +177,7 @@
  * Warning: This is not an official time (so it cannot be used to dispute ref
  * calls or guarantee that a function will trigger before the match ends).
  *
- * The Practice Match function of the DS approximates the behaviour seen on
+ * The Practice Match function of the DS approximates the behavior seen on
  * the field.
  *
  * @param status the error code, or 0 for success
@@ -194,24 +200,6 @@
 void HAL_ReleaseDSMutex(void);
 
 /**
- * Checks if new control data has arrived since the last
- * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has
- * not arrived, waits for new data to arrive. Otherwise, returns immediately.
- */
-void HAL_WaitForCachedControlData(void);
-
-/**
- * Checks if new control data has arrived since the last
- * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has
- * not arrived, waits for new data to arrive, or a timeout. Otherwise, returns
- * immediately.
- *
- * @param timeout timeout in seconds
- * @return        true for new data, false for timeout
- */
-HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout);
-
-/**
  * Has a new control packet from the driver station arrived since the last
  * time this function was called?
  *
@@ -221,6 +209,9 @@
 
 /**
  * Waits for the newest DS packet to arrive. Note that this is a blocking call.
+ * Checks if new control data has arrived since the last HAL_WaitForDSData or
+ * HAL_IsNewControlData call. If new data has not arrived, waits for new data
+ * to arrive. Otherwise, returns immediately.
  */
 void HAL_WaitForDSData(void);
 

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

diff --git a/hal/src/main/native/include/hal/Extensions.h b/hal/src/main/native/include/hal/Extensions.h
index 3a435c0..13de7f8 100644
--- a/hal/src/main/native/include/hal/Extensions.h
+++ b/hal/src/main/native/include/hal/Extensions.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,7 +25,10 @@
  */
 typedef int halsim_extension_init_func_t(void);
 
+#ifdef __cplusplus
 extern "C" {
+#endif
+
 /**
  * Loads a single extension from a direct path.
  *
@@ -45,6 +48,31 @@
 int HAL_LoadExtensions(void);
 
 /**
+ * Registers an extension such that other extensions can discover it.
+ *
+ * The passed data pointer is retained and the extension must keep this
+ * pointer valid.
+ *
+ * @param name extension name (may embed version number)
+ * @param data data pointer
+ */
+void HAL_RegisterExtension(const char* name, void* data);
+
+/**
+ * Registers an extension registration listener function. The function will
+ * be called immediately with any currently registered extensions, and will
+ * be called later when any additional extensions are registered.
+ *
+ * @param param parameter data to pass to callback function
+ * @param func callback function to be called for each registered extension;
+ *             parameters are the parameter data, extension name, and extension
+ *             data pointer passed to HAL_RegisterExtension()
+ */
+void HAL_RegisterExtensionListener(void* param,
+                                   void (*func)(void*, const char* name,
+                                                void* data));
+
+/**
  * Enables or disables the message saying no HAL extensions were found.
  *
  * Some apps don't care, and the message create clutter. For general team code,
@@ -57,5 +85,17 @@
  * @param showMessage true to show message, false to not.
  */
 void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage);
+
+/**
+ * Registers a function to be called from HAL_Shutdown(). This is intended
+ * for use only by simulation extensions.
+ *
+ * @param param parameter data to pass to callback function
+ * @param func callback function
+ */
+void HAL_OnShutdown(void* param, void (*func)(void*));
+
+#ifdef __cplusplus
 }  // extern "C"
+#endif
 /** @} */

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

diff --git a/hal/src/main/native/include/hal/HAL.h b/hal/src/main/native/include/hal/HAL.h
index cf9b2f8..4412886 100644
--- a/hal/src/main/native/include/hal/HAL.h
+++ b/hal/src/main/native/include/hal/HAL.h
@@ -11,6 +11,7 @@
 
 #include "hal/Accelerometer.h"
 #include "hal/AnalogAccumulator.h"
+#include "hal/AnalogGyro.h"
 #include "hal/AnalogInput.h"
 #include "hal/AnalogOutput.h"
 #include "hal/AnalogTrigger.h"
@@ -35,6 +36,7 @@
 #include "hal/Power.h"
 #include "hal/Relay.h"
 #include "hal/SPI.h"
+#include "hal/SerialPort.h"
 #include "hal/SimDevice.h"
 #include "hal/Solenoid.h"
 #include "hal/Threads.h"

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

diff --git a/hal/src/main/native/include/hal/HALBase.h b/hal/src/main/native/include/hal/HALBase.h
index f61d9e4..ee5b054 100644
--- a/hal/src/main/native/include/hal/HALBase.h
+++ b/hal/src/main/native/include/hal/HALBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -149,6 +149,14 @@
  */
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode);
 
+/**
+ * Call this to shut down HAL.
+ *
+ * This must be called at termination of the robot program to avoid potential
+ * segmentation faults with simulation extensions at exit.
+ */
+void HAL_Shutdown(void);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif

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

diff --git a/hal/src/main/native/include/hal/I2C.h b/hal/src/main/native/include/hal/I2C.h
index fc8b5c8..4147cbb 100644
--- a/hal/src/main/native/include/hal/I2C.h
+++ b/hal/src/main/native/include/hal/I2C.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 #include <stdint.h>
 
 #include "hal/I2CTypes.h"
-#include "hal/Types.h"
 
 /**
  * @defgroup hal_i2c I2C Functions

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

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

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

diff --git a/hal/src/main/native/include/hal/Main.h b/hal/src/main/native/include/hal/Main.h
index 866e274..097f819 100644
--- a/hal/src/main/native/include/hal/Main.h
+++ b/hal/src/main/native/include/hal/Main.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,6 @@
 
 #pragma once
 
-#include <stdint.h>
-
 #include "hal/Types.h"
 
 /**

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

diff --git a/hal/src/main/native/include/hal/PWM.h b/hal/src/main/native/include/hal/PWM.h
index 781a423..7267823 100644
--- a/hal/src/main/native/include/hal/PWM.h
+++ b/hal/src/main/native/include/hal/PWM.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -158,7 +158,7 @@
  * Sets a PWM channel to be disabled.
  *
  * The channel is disabled until the next time it is set. Note this is different
- * from just setting a 0 speed, as this will actively stop all signalling on the
+ * from just setting a 0 speed, as this will actively stop all signaling on the
  * channel.
  *
  * @param pwmPortHandle the PWM handle.

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

diff --git a/hal/src/main/native/include/hal/SimDevice.h b/hal/src/main/native/include/hal/SimDevice.h
index b05021e..ff4c12e 100644
--- a/hal/src/main/native/include/hal/SimDevice.h
+++ b/hal/src/main/native/include/hal/SimDevice.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -473,14 +473,6 @@
    */
   SimDevice(const char* name, int index, int channel);
 
-  /**
-   * Wraps a simulated device handle as returned by HAL_CreateSimDevice().
-   *
-   * @param handle simulated device handle
-   */
-  /*implicit*/ SimDevice(HAL_SimDeviceHandle val)  // NOLINT(runtime/explicit)
-      : m_handle(val) {}
-
   ~SimDevice() {
     if (m_handle != HAL_kInvalidHandle) HAL_FreeSimDevice(m_handle);
   }

hal/src/main/native/include/hal/cpp/fpga_clock.h

diff --git a/hal/src/main/native/include/hal/cpp/fpga_clock.h b/hal/src/main/native/include/hal/cpp/fpga_clock.h
index 94031b1..f6d5c6c 100644
--- a/hal/src/main/native/include/hal/cpp/fpga_clock.h
+++ b/hal/src/main/native/include/hal/cpp/fpga_clock.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,6 @@
 #pragma once
 
 #include <chrono>
-#include <limits>
 
 /** WPILib Hardware Abstraction Layer (HAL) namespace */
 namespace hal {

hal/src/main/native/include/hal/handles/DigitalHandleResource.h

diff --git a/hal/src/main/native/include/hal/handles/DigitalHandleResource.h b/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
index dcd4b97..5fd8506 100644
--- a/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
+++ b/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,7 +23,7 @@
 /**
  * The DigitalHandleResource class is a way to track handles. This version
  * allows a limited number of handles that are allocated by index.
- * The enum value is seperate, as 2 enum values are allowed per handle
+ * The enum value is separate, as 2 enum values are allowed per handle
  * Because they are allocated by index, each individual index holds its own
  * mutex, which reduces contention heavily.]
  *
@@ -42,6 +42,9 @@
   DigitalHandleResource& operator=(const DigitalHandleResource&) = delete;
 
   THandle Allocate(int16_t index, HAL_HandleEnum enumValue, int32_t* status);
+  int16_t GetIndex(THandle handle, HAL_HandleEnum enumValue) {
+    return getHandleTypedIndex(handle, enumValue, m_version);
+  }
   std::shared_ptr<TStruct> Get(THandle handle, HAL_HandleEnum enumValue);
   void Free(THandle handle, HAL_HandleEnum enumValue);
   void ResetHandles() override;
@@ -54,7 +57,7 @@
 template <typename THandle, typename TStruct, int16_t size>
 THandle DigitalHandleResource<THandle, TStruct, size>::Allocate(
     int16_t index, HAL_HandleEnum enumValue, int32_t* status) {
-  // don't aquire the lock if we can fail early.
+  // don't acquire the lock if we can fail early.
   if (index < 0 || index >= size) {
     *status = RESOURCE_OUT_OF_RANGE;
     return HAL_kInvalidHandle;
@@ -73,12 +76,12 @@
 std::shared_ptr<TStruct> DigitalHandleResource<THandle, TStruct, size>::Get(
     THandle handle, HAL_HandleEnum enumValue) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle, enumValue);
   if (index < 0 || index >= size) {
     return nullptr;
   }
   std::scoped_lock lock(m_handleMutexes[index]);
-  // return structure. Null will propogate correctly, so no need to manually
+  // return structure. Null will propagate correctly, so no need to manually
   // check.
   return m_structures[index];
 }
@@ -87,7 +90,7 @@
 void DigitalHandleResource<THandle, TStruct, size>::Free(
     THandle handle, HAL_HandleEnum enumValue) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle, enumValue);
   if (index < 0 || index >= size) return;
   // lock and deallocated handle
   std::scoped_lock lock(m_handleMutexes[index]);

hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h

diff --git a/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h b/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
index 2725573..902f023 100644
--- a/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
+++ b/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -47,6 +47,9 @@
 
   THandle Allocate(int16_t index, std::shared_ptr<TStruct> toSet,
                    int32_t* status);
+  int16_t GetIndex(THandle handle) {
+    return getHandleTypedIndex(handle, enumValue, m_version);
+  }
   std::shared_ptr<TStruct> Get(THandle handle);
   void Free(THandle handle);
   void ResetHandles();
@@ -61,7 +64,7 @@
 THandle
 IndexedClassedHandleResource<THandle, TStruct, size, enumValue>::Allocate(
     int16_t index, std::shared_ptr<TStruct> toSet, int32_t* status) {
-  // don't aquire the lock if we can fail early.
+  // don't acquire the lock if we can fail early.
   if (index < 0 || index >= size) {
     *status = RESOURCE_OUT_OF_RANGE;
     return HAL_kInvalidHandle;
@@ -82,12 +85,12 @@
 IndexedClassedHandleResource<THandle, TStruct, size, enumValue>::Get(
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) {
     return nullptr;
   }
   std::scoped_lock lock(m_handleMutexes[index]);
-  // return structure. Null will propogate correctly, so no need to manually
+  // return structure. Null will propagate correctly, so no need to manually
   // check.
   return m_structures[index];
 }
@@ -97,7 +100,7 @@
 void IndexedClassedHandleResource<THandle, TStruct, size, enumValue>::Free(
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) return;
   // lock and deallocated handle
   std::scoped_lock lock(m_handleMutexes[index]);

hal/src/main/native/include/hal/handles/IndexedHandleResource.h

diff --git a/hal/src/main/native/include/hal/handles/IndexedHandleResource.h b/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
index 2bca4ce..26fda02 100644
--- a/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
+++ b/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -43,6 +43,9 @@
   IndexedHandleResource& operator=(const IndexedHandleResource&) = delete;
 
   THandle Allocate(int16_t index, int32_t* status);
+  int16_t GetIndex(THandle handle) {
+    return getHandleTypedIndex(handle, enumValue, m_version);
+  }
   std::shared_ptr<TStruct> Get(THandle handle);
   void Free(THandle handle);
   void ResetHandles() override;
@@ -56,7 +59,7 @@
           HAL_HandleEnum enumValue>
 THandle IndexedHandleResource<THandle, TStruct, size, enumValue>::Allocate(
     int16_t index, int32_t* status) {
-  // don't aquire the lock if we can fail early.
+  // don't acquire the lock if we can fail early.
   if (index < 0 || index >= size) {
     *status = RESOURCE_OUT_OF_RANGE;
     return HAL_kInvalidHandle;
@@ -76,12 +79,12 @@
 std::shared_ptr<TStruct>
 IndexedHandleResource<THandle, TStruct, size, enumValue>::Get(THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) {
     return nullptr;
   }
   std::scoped_lock lock(m_handleMutexes[index]);
-  // return structure. Null will propogate correctly, so no need to manually
+  // return structure. Null will propagate correctly, so no need to manually
   // check.
   return m_structures[index];
 }
@@ -91,7 +94,7 @@
 void IndexedHandleResource<THandle, TStruct, size, enumValue>::Free(
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) return;
   // lock and deallocated handle
   std::scoped_lock lock(m_handleMutexes[index]);

hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h

diff --git a/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h b/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
index a991fc3..0ab8aac 100644
--- a/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
+++ b/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -42,6 +42,9 @@
       delete;
 
   THandle Allocate(std::shared_ptr<TStruct> toSet);
+  int16_t GetIndex(THandle handle) {
+    return getHandleTypedIndex(handle, enumValue, m_version);
+  }
   std::shared_ptr<TStruct> Get(THandle handle);
   void Free(THandle handle);
   void ResetHandles() override;
@@ -77,12 +80,12 @@
 LimitedClassedHandleResource<THandle, TStruct, size, enumValue>::Get(
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) {
     return nullptr;
   }
   std::scoped_lock lock(m_handleMutexes[index]);
-  // return structure. Null will propogate correctly, so no need to manually
+  // return structure. Null will propagate correctly, so no need to manually
   // check.
   return m_structures[index];
 }
@@ -92,7 +95,7 @@
 void LimitedClassedHandleResource<THandle, TStruct, size, enumValue>::Free(
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) return;
   // lock and deallocated handle
   std::scoped_lock allocateLock(m_allocateMutex);

hal/src/main/native/include/hal/handles/LimitedHandleResource.h

diff --git a/hal/src/main/native/include/hal/handles/LimitedHandleResource.h b/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
index 0756634..2f7ed0d 100644
--- a/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
+++ b/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -40,6 +40,9 @@
   LimitedHandleResource& operator=(const LimitedHandleResource&) = delete;
 
   THandle Allocate();
+  int16_t GetIndex(THandle handle) {
+    return getHandleTypedIndex(handle, enumValue, m_version);
+  }
   std::shared_ptr<TStruct> Get(THandle handle);
   void Free(THandle handle);
   void ResetHandles() override;
@@ -72,12 +75,12 @@
 std::shared_ptr<TStruct>
 LimitedHandleResource<THandle, TStruct, size, enumValue>::Get(THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) {
     return nullptr;
   }
   std::scoped_lock lock(m_handleMutexes[index]);
-  // return structure. Null will propogate correctly, so no need to manually
+  // return structure. Null will propagate correctly, so no need to manually
   // check.
   return m_structures[index];
 }
@@ -87,7 +90,7 @@
 void LimitedHandleResource<THandle, TStruct, size, enumValue>::Free(
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   if (index < 0 || index >= size) return;
   // lock and deallocated handle
   std::scoped_lock allocateLock(m_allocateMutex);

hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h

diff --git a/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h b/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
index 96a91f8..5f74b88 100644
--- a/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
+++ b/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -44,6 +44,9 @@
   UnlimitedHandleResource& operator=(const UnlimitedHandleResource&) = delete;
 
   THandle Allocate(std::shared_ptr<TStruct> structure);
+  int16_t GetIndex(THandle handle) {
+    return getHandleTypedIndex(handle, enumValue, m_version);
+  }
   std::shared_ptr<TStruct> Get(THandle handle);
   /* Returns structure previously at that handle (or nullptr if none) */
   std::shared_ptr<TStruct> Free(THandle handle);
@@ -81,7 +84,7 @@
 template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
 std::shared_ptr<TStruct>
 UnlimitedHandleResource<THandle, TStruct, enumValue>::Get(THandle handle) {
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   std::scoped_lock lock(m_handleMutex);
   if (index < 0 || index >= static_cast<int16_t>(m_structures.size()))
     return nullptr;
@@ -91,7 +94,7 @@
 template <typename THandle, typename TStruct, HAL_HandleEnum enumValue>
 std::shared_ptr<TStruct>
 UnlimitedHandleResource<THandle, TStruct, enumValue>::Free(THandle handle) {
-  int16_t index = getHandleTypedIndex(handle, enumValue, m_version);
+  int16_t index = GetIndex(handle);
   std::scoped_lock lock(m_handleMutex);
   if (index < 0 || index >= static_cast<int16_t>(m_structures.size()))
     return nullptr;

hal/src/main/native/include/hal/simulation/AccelerometerData.h

diff --git a/hal/src/main/native/include/hal/simulation/AccelerometerData.h b/hal/src/main/native/include/hal/simulation/AccelerometerData.h
new file mode 100644
index 0000000..0a92671
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/AccelerometerData.h
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Accelerometer.h"
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAccelerometerData(int32_t index);
+int32_t HALSIM_RegisterAccelerometerActiveCallback(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerActiveCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAccelerometerActive(int32_t index);
+void HALSIM_SetAccelerometerActive(int32_t index, HAL_Bool active);
+
+int32_t HALSIM_RegisterAccelerometerRangeCallback(int32_t index,
+                                                  HAL_NotifyCallback callback,
+                                                  void* param,
+                                                  HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerRangeCallback(int32_t index, int32_t uid);
+HAL_AccelerometerRange HALSIM_GetAccelerometerRange(int32_t index);
+void HALSIM_SetAccelerometerRange(int32_t index, HAL_AccelerometerRange range);
+
+int32_t HALSIM_RegisterAccelerometerXCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerXCallback(int32_t index, int32_t uid);
+double HALSIM_GetAccelerometerX(int32_t index);
+void HALSIM_SetAccelerometerX(int32_t index, double x);
+
+int32_t HALSIM_RegisterAccelerometerYCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerYCallback(int32_t index, int32_t uid);
+double HALSIM_GetAccelerometerY(int32_t index);
+void HALSIM_SetAccelerometerY(int32_t index, double y);
+
+int32_t HALSIM_RegisterAccelerometerZCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelAccelerometerZCallback(int32_t index, int32_t uid);
+double HALSIM_GetAccelerometerZ(int32_t index);
+void HALSIM_SetAccelerometerZ(int32_t index, double z);
+
+void HALSIM_RegisterAccelerometerAllCallbacks(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/AddressableLEDData.h

diff --git a/hal/src/main/native/include/hal/simulation/AddressableLEDData.h b/hal/src/main/native/include/hal/simulation/AddressableLEDData.h
new file mode 100644
index 0000000..91ab30c
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/AddressableLEDData.h
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/AddressableLEDTypes.h"
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel);
+
+void HALSIM_ResetAddressableLEDData(int32_t index);
+
+int32_t HALSIM_RegisterAddressableLEDInitializedCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAddressableLEDInitialized(int32_t index);
+void HALSIM_SetAddressableLEDInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterAddressableLEDOutputPortCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDOutputPortCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDOutputPort(int32_t index);
+void HALSIM_SetAddressableLEDOutputPort(int32_t index, int32_t outputPort);
+
+int32_t HALSIM_RegisterAddressableLEDLengthCallback(int32_t index,
+                                                    HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDLengthCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDLength(int32_t index);
+void HALSIM_SetAddressableLEDLength(int32_t index, int32_t length);
+
+int32_t HALSIM_RegisterAddressableLEDRunningCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAddressableLEDRunningCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAddressableLEDRunning(int32_t index);
+void HALSIM_SetAddressableLEDRunning(int32_t index, HAL_Bool running);
+
+int32_t HALSIM_RegisterAddressableLEDDataCallback(
+    int32_t index, HAL_ConstBufferCallback callback, void* param);
+void HALSIM_CancelAddressableLEDDataCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAddressableLEDData(int32_t index,
+                                     struct HAL_AddressableLEDData* data);
+void HALSIM_SetAddressableLEDData(int32_t index,
+                                  const struct HAL_AddressableLEDData* data,
+                                  int32_t length);
+
+void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/AnalogGyroData.h

diff --git a/hal/src/main/native/include/hal/simulation/AnalogGyroData.h b/hal/src/main/native/include/hal/simulation/AnalogGyroData.h
new file mode 100644
index 0000000..91e684e
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/AnalogGyroData.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAnalogGyroData(int32_t index);
+int32_t HALSIM_RegisterAnalogGyroAngleCallback(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+void HALSIM_CancelAnalogGyroAngleCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogGyroAngle(int32_t index);
+void HALSIM_SetAnalogGyroAngle(int32_t index, double angle);
+
+int32_t HALSIM_RegisterAnalogGyroRateCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelAnalogGyroRateCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogGyroRate(int32_t index);
+void HALSIM_SetAnalogGyroRate(int32_t index, double rate);
+
+int32_t HALSIM_RegisterAnalogGyroInitializedCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogGyroInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogGyroInitialized(int32_t index);
+void HALSIM_SetAnalogGyroInitialized(int32_t index, HAL_Bool initialized);
+
+void HALSIM_RegisterAnalogGyroAllCallbacks(int32_t index,
+                                           HAL_NotifyCallback callback,
+                                           void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/AnalogInData.h

diff --git a/hal/src/main/native/include/hal/simulation/AnalogInData.h b/hal/src/main/native/include/hal/simulation/AnalogInData.h
new file mode 100644
index 0000000..9ab2d3b
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/AnalogInData.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAnalogInData(int32_t index);
+int32_t HALSIM_RegisterAnalogInInitializedCallback(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogInInitialized(int32_t index);
+void HALSIM_SetAnalogInInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterAnalogInAverageBitsCallback(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAverageBitsCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAnalogInAverageBits(int32_t index);
+void HALSIM_SetAnalogInAverageBits(int32_t index, int32_t averageBits);
+
+int32_t HALSIM_RegisterAnalogInOversampleBitsCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInOversampleBitsCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAnalogInOversampleBits(int32_t index);
+void HALSIM_SetAnalogInOversampleBits(int32_t index, int32_t oversampleBits);
+
+int32_t HALSIM_RegisterAnalogInVoltageCallback(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogInVoltage(int32_t index);
+void HALSIM_SetAnalogInVoltage(int32_t index, double voltage);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorInitializedCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorInitializedCallback(int32_t index,
+                                                         int32_t uid);
+HAL_Bool HALSIM_GetAnalogInAccumulatorInitialized(int32_t index);
+void HALSIM_SetAnalogInAccumulatorInitialized(int32_t index,
+                                              HAL_Bool accumulatorInitialized);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorValueCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorValueCallback(int32_t index, int32_t uid);
+int64_t HALSIM_GetAnalogInAccumulatorValue(int32_t index);
+void HALSIM_SetAnalogInAccumulatorValue(int32_t index,
+                                        int64_t accumulatorValue);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorCountCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorCountCallback(int32_t index, int32_t uid);
+int64_t HALSIM_GetAnalogInAccumulatorCount(int32_t index);
+void HALSIM_SetAnalogInAccumulatorCount(int32_t index,
+                                        int64_t accumulatorCount);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorCenterCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorCenterCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetAnalogInAccumulatorCenter(int32_t index);
+void HALSIM_SetAnalogInAccumulatorCenter(int32_t index,
+                                         int32_t accumulatorCenter);
+
+int32_t HALSIM_RegisterAnalogInAccumulatorDeadbandCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogInAccumulatorDeadbandCallback(int32_t index,
+                                                      int32_t uid);
+int32_t HALSIM_GetAnalogInAccumulatorDeadband(int32_t index);
+void HALSIM_SetAnalogInAccumulatorDeadband(int32_t index,
+                                           int32_t accumulatorDeadband);
+
+void HALSIM_RegisterAnalogInAllCallbacks(int32_t index,
+                                         HAL_NotifyCallback callback,
+                                         void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/AnalogOutData.h

diff --git a/hal/src/main/native/include/hal/simulation/AnalogOutData.h b/hal/src/main/native/include/hal/simulation/AnalogOutData.h
new file mode 100644
index 0000000..fa1413d
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/AnalogOutData.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetAnalogOutData(int32_t index);
+int32_t HALSIM_RegisterAnalogOutVoltageCallback(int32_t index,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify);
+void HALSIM_CancelAnalogOutVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetAnalogOutVoltage(int32_t index);
+void HALSIM_SetAnalogOutVoltage(int32_t index, double voltage);
+
+int32_t HALSIM_RegisterAnalogOutInitializedCallback(int32_t index,
+                                                    HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogOutInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogOutInitialized(int32_t index);
+void HALSIM_SetAnalogOutInitialized(int32_t index, HAL_Bool initialized);
+
+void HALSIM_RegisterAnalogOutAllCallbacks(int32_t index,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/AnalogTriggerData.h

diff --git a/hal/src/main/native/include/hal/simulation/AnalogTriggerData.h b/hal/src/main/native/include/hal/simulation/AnalogTriggerData.h
new file mode 100644
index 0000000..74c762c
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/AnalogTriggerData.h
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+enum HALSIM_AnalogTriggerMode : int32_t {
+  HALSIM_AnalogTriggerUnassigned,
+  HALSIM_AnalogTriggerFiltered,
+  HALSIM_AnalogTriggerDutyCycle,
+  HALSIM_AnalogTriggerAveraged
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel);
+
+void HALSIM_ResetAnalogTriggerData(int32_t index);
+int32_t HALSIM_RegisterAnalogTriggerInitializedCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetAnalogTriggerInitialized(int32_t index);
+void HALSIM_SetAnalogTriggerInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback(int32_t index,
+                                                         int32_t uid);
+double HALSIM_GetAnalogTriggerTriggerLowerBound(int32_t index);
+void HALSIM_SetAnalogTriggerTriggerLowerBound(int32_t index,
+                                              double triggerLowerBound);
+
+int32_t HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback(int32_t index,
+                                                         int32_t uid);
+double HALSIM_GetAnalogTriggerTriggerUpperBound(int32_t index);
+void HALSIM_SetAnalogTriggerTriggerUpperBound(int32_t index,
+                                              double triggerUpperBound);
+
+int32_t HALSIM_RegisterAnalogTriggerTriggerModeCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelAnalogTriggerTriggerModeCallback(int32_t index, int32_t uid);
+HALSIM_AnalogTriggerMode HALSIM_GetAnalogTriggerTriggerMode(int32_t index);
+void HALSIM_SetAnalogTriggerTriggerMode(int32_t index,
+                                        HALSIM_AnalogTriggerMode triggerMode);
+
+void HALSIM_RegisterAnalogTriggerAllCallbacks(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/CanData.h

diff --git a/hal/src/main/native/include/hal/simulation/CanData.h b/hal/src/main/native/include/hal/simulation/CanData.h
new file mode 100644
index 0000000..eb5ea63
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/CanData.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/Value.h"
+#include "hal/simulation/NotifyListener.h"
+
+typedef void (*HAL_CAN_SendMessageCallback)(const char* name, void* param,
+                                            uint32_t messageID,
+                                            const uint8_t* data,
+                                            uint8_t dataSize, int32_t periodMs,
+                                            int32_t* status);
+
+typedef void (*HAL_CAN_ReceiveMessageCallback)(
+    const char* name, void* param, uint32_t* messageID, uint32_t messageIDMask,
+    uint8_t* data, uint8_t* dataSize, uint32_t* timeStamp, int32_t* status);
+
+typedef void (*HAL_CAN_OpenStreamSessionCallback)(
+    const char* name, void* param, uint32_t* sessionHandle, uint32_t messageID,
+    uint32_t messageIDMask, uint32_t maxMessages, int32_t* status);
+
+typedef void (*HAL_CAN_CloseStreamSessionCallback)(const char* name,
+                                                   void* param,
+                                                   uint32_t sessionHandle);
+
+typedef void (*HAL_CAN_ReadStreamSessionCallback)(
+    const char* name, void* param, uint32_t sessionHandle,
+    struct HAL_CANStreamMessage* messages, uint32_t messagesToRead,
+    uint32_t* messagesRead, int32_t* status);
+
+typedef void (*HAL_CAN_GetCANStatusCallback)(
+    const char* name, void* param, float* percentBusUtilization,
+    uint32_t* busOffCount, uint32_t* txFullCount, uint32_t* receiveErrorCount,
+    uint32_t* transmitErrorCount, int32_t* status);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetCanData(void);
+
+int32_t HALSIM_RegisterCanSendMessageCallback(
+    HAL_CAN_SendMessageCallback callback, void* param);
+void HALSIM_CancelCanSendMessageCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanReceiveMessageCallback(
+    HAL_CAN_ReceiveMessageCallback callback, void* param);
+void HALSIM_CancelCanReceiveMessageCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanOpenStreamCallback(
+    HAL_CAN_OpenStreamSessionCallback callback, void* param);
+void HALSIM_CancelCanOpenStreamCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanCloseStreamCallback(
+    HAL_CAN_CloseStreamSessionCallback callback, void* param);
+void HALSIM_CancelCanCloseStreamCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanReadStreamCallback(
+    HAL_CAN_ReadStreamSessionCallback callback, void* param);
+void HALSIM_CancelCanReadStreamCallback(int32_t uid);
+
+int32_t HALSIM_RegisterCanGetCANStatusCallback(
+    HAL_CAN_GetCANStatusCallback callback, void* param);
+void HALSIM_CancelCanGetCANStatusCallback(int32_t uid);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/DIOData.h

diff --git a/hal/src/main/native/include/hal/simulation/DIOData.h b/hal/src/main/native/include/hal/simulation/DIOData.h
new file mode 100644
index 0000000..ddda655
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/DIOData.h
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetDIOData(int32_t index);
+int32_t HALSIM_RegisterDIOInitializedCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelDIOInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDIOInitialized(int32_t index);
+void HALSIM_SetDIOInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterDIOValueCallback(int32_t index,
+                                        HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDIOValueCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDIOValue(int32_t index);
+void HALSIM_SetDIOValue(int32_t index, HAL_Bool value);
+
+int32_t HALSIM_RegisterDIOPulseLengthCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelDIOPulseLengthCallback(int32_t index, int32_t uid);
+double HALSIM_GetDIOPulseLength(int32_t index);
+void HALSIM_SetDIOPulseLength(int32_t index, double pulseLength);
+
+int32_t HALSIM_RegisterDIOIsInputCallback(int32_t index,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDIOIsInputCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDIOIsInput(int32_t index);
+void HALSIM_SetDIOIsInput(int32_t index, HAL_Bool isInput);
+
+int32_t HALSIM_RegisterDIOFilterIndexCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelDIOFilterIndexCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetDIOFilterIndex(int32_t index);
+void HALSIM_SetDIOFilterIndex(int32_t index, int32_t filterIndex);
+
+void HALSIM_RegisterDIOAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+                                    void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/DigitalPWMData.h

diff --git a/hal/src/main/native/include/hal/simulation/DigitalPWMData.h b/hal/src/main/native/include/hal/simulation/DigitalPWMData.h
new file mode 100644
index 0000000..79428b0
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/DigitalPWMData.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel);
+
+void HALSIM_ResetDigitalPWMData(int32_t index);
+int32_t HALSIM_RegisterDigitalPWMInitializedCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelDigitalPWMInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDigitalPWMInitialized(int32_t index);
+void HALSIM_SetDigitalPWMInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterDigitalPWMDutyCycleCallback(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelDigitalPWMDutyCycleCallback(int32_t index, int32_t uid);
+double HALSIM_GetDigitalPWMDutyCycle(int32_t index);
+void HALSIM_SetDigitalPWMDutyCycle(int32_t index, double dutyCycle);
+
+int32_t HALSIM_RegisterDigitalPWMPinCallback(int32_t index,
+                                             HAL_NotifyCallback callback,
+                                             void* param,
+                                             HAL_Bool initialNotify);
+void HALSIM_CancelDigitalPWMPinCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetDigitalPWMPin(int32_t index);
+void HALSIM_SetDigitalPWMPin(int32_t index, int32_t pin);
+
+void HALSIM_RegisterDigitalPWMAllCallbacks(int32_t index,
+                                           HAL_NotifyCallback callback,
+                                           void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/DriverStationData.h

diff --git a/hal/src/main/native/include/hal/simulation/DriverStationData.h b/hal/src/main/native/include/hal/simulation/DriverStationData.h
new file mode 100644
index 0000000..536738b
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/DriverStationData.h
@@ -0,0 +1,172 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/DriverStationTypes.h"
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+typedef void (*HAL_JoystickAxesCallback)(const char* name, void* param,
+                                         int32_t joystickNum,
+                                         const HAL_JoystickAxes* axes);
+typedef void (*HAL_JoystickPOVsCallback)(const char* name, void* param,
+                                         int32_t joystickNum,
+                                         const HAL_JoystickPOVs* povs);
+typedef void (*HAL_JoystickButtonsCallback)(const char* name, void* param,
+                                            int32_t joystickNum,
+                                            const HAL_JoystickButtons* buttons);
+typedef void (*HAL_JoystickDescriptorCallback)(
+    const char* name, void* param, int32_t joystickNum,
+    const HAL_JoystickDescriptor* descriptor);
+typedef void (*HAL_JoystickOutputsCallback)(const char* name, void* param,
+                                            int32_t joystickNum,
+                                            int64_t outputs, int32_t leftRumble,
+                                            int32_t rightRumble);
+typedef void (*HAL_MatchInfoCallback)(const char* name, void* param,
+                                      const HAL_MatchInfo* info);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetDriverStationData(void);
+int32_t HALSIM_RegisterDriverStationEnabledCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationEnabledCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationEnabled(void);
+void HALSIM_SetDriverStationEnabled(HAL_Bool enabled);
+
+int32_t HALSIM_RegisterDriverStationAutonomousCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationAutonomousCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationAutonomous(void);
+void HALSIM_SetDriverStationAutonomous(HAL_Bool autonomous);
+
+int32_t HALSIM_RegisterDriverStationTestCallback(HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationTestCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationTest(void);
+void HALSIM_SetDriverStationTest(HAL_Bool test);
+
+int32_t HALSIM_RegisterDriverStationEStopCallback(HAL_NotifyCallback callback,
+                                                  void* param,
+                                                  HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationEStopCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationEStop(void);
+void HALSIM_SetDriverStationEStop(HAL_Bool eStop);
+
+int32_t HALSIM_RegisterDriverStationFmsAttachedCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationFmsAttachedCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationFmsAttached(void);
+void HALSIM_SetDriverStationFmsAttached(HAL_Bool fmsAttached);
+
+int32_t HALSIM_RegisterDriverStationDsAttachedCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationDsAttachedCallback(int32_t uid);
+HAL_Bool HALSIM_GetDriverStationDsAttached(void);
+void HALSIM_SetDriverStationDsAttached(HAL_Bool dsAttached);
+
+int32_t HALSIM_RegisterDriverStationAllianceStationIdCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationAllianceStationIdCallback(int32_t uid);
+HAL_AllianceStationID HALSIM_GetDriverStationAllianceStationId(void);
+void HALSIM_SetDriverStationAllianceStationId(
+    HAL_AllianceStationID allianceStationId);
+
+int32_t HALSIM_RegisterDriverStationMatchTimeCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationMatchTimeCallback(int32_t uid);
+double HALSIM_GetDriverStationMatchTime(void);
+void HALSIM_SetDriverStationMatchTime(double matchTime);
+
+int32_t HALSIM_RegisterJoystickAxesCallback(int32_t joystickNum,
+                                            HAL_JoystickAxesCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+void HALSIM_CancelJoystickAxesCallback(int32_t uid);
+void HALSIM_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes);
+void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes);
+
+int32_t HALSIM_RegisterJoystickPOVsCallback(int32_t joystickNum,
+                                            HAL_JoystickPOVsCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+void HALSIM_CancelJoystickPOVsCallback(int32_t uid);
+void HALSIM_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs);
+void HALSIM_SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs);
+
+int32_t HALSIM_RegisterJoystickButtonsCallback(
+    int32_t joystickNum, HAL_JoystickButtonsCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelJoystickButtonsCallback(int32_t uid);
+void HALSIM_GetJoystickButtons(int32_t joystickNum,
+                               HAL_JoystickButtons* buttons);
+void HALSIM_SetJoystickButtons(int32_t joystickNum,
+                               const HAL_JoystickButtons* buttons);
+
+int32_t HALSIM_RegisterJoystickDescriptorCallback(
+    int32_t joystickNum, HAL_JoystickDescriptorCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelJoystickDescriptorCallback(int32_t uid);
+void HALSIM_GetJoystickDescriptor(int32_t joystickNum,
+                                  HAL_JoystickDescriptor* descriptor);
+void HALSIM_SetJoystickDescriptor(int32_t joystickNum,
+                                  const HAL_JoystickDescriptor* descriptor);
+
+int32_t HALSIM_RegisterJoystickOutputsCallback(
+    int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelJoystickOutputsCallback(int32_t uid);
+void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
+                               int32_t* leftRumble, int32_t* rightRumble);
+void HALSIM_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+                               int32_t leftRumble, int32_t rightRumble);
+
+int32_t HALSIM_RegisterMatchInfoCallback(HAL_MatchInfoCallback callback,
+                                         void* param, HAL_Bool initialNotify);
+void HALSIM_CancelMatchInfoCallback(int32_t uid);
+void HALSIM_GetMatchInfo(HAL_MatchInfo* info);
+void HALSIM_SetMatchInfo(const HAL_MatchInfo* info);
+
+void HALSIM_SetJoystickButton(int32_t stick, int32_t button, HAL_Bool state);
+void HALSIM_SetJoystickAxis(int32_t stick, int32_t axis, double value);
+void HALSIM_SetJoystickPOV(int32_t stick, int32_t pov, int32_t value);
+void HALSIM_SetJoystickButtonsValue(int32_t stick, uint32_t buttons);
+void HALSIM_SetJoystickAxisCount(int32_t stick, int32_t count);
+void HALSIM_SetJoystickPOVCount(int32_t stick, int32_t count);
+void HALSIM_SetJoystickButtonCount(int32_t stick, int32_t count);
+void HALSIM_GetJoystickCounts(int32_t stick, int32_t* axisCount,
+                              int32_t* buttonCount, int32_t* povCount);
+
+void HALSIM_SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox);
+void HALSIM_SetJoystickType(int32_t stick, int32_t type);
+void HALSIM_SetJoystickName(int32_t stick, const char* name);
+void HALSIM_SetJoystickAxisType(int32_t stick, int32_t axis, int32_t type);
+
+void HALSIM_SetGameSpecificMessage(const char* message);
+void HALSIM_SetEventName(const char* name);
+void HALSIM_SetMatchType(HAL_MatchType type);
+void HALSIM_SetMatchNumber(int32_t matchNumber);
+void HALSIM_SetReplayNumber(int32_t replayNumber);
+
+void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+
+int32_t HALSIM_RegisterDriverStationNewDataCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelDriverStationNewDataCallback(int32_t uid);
+void HALSIM_NotifyDriverStationNewData(void);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/DutyCycleData.h

diff --git a/hal/src/main/native/include/hal/simulation/DutyCycleData.h b/hal/src/main/native/include/hal/simulation/DutyCycleData.h
new file mode 100644
index 0000000..7b191f28
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/DutyCycleData.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int32_t HALSIM_FindDutyCycleForChannel(int32_t channel);
+
+void HALSIM_ResetDutyCycleData(int32_t index);
+int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index);
+
+int32_t HALSIM_RegisterDutyCycleInitializedCallback(int32_t index,
+                                                    HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetDutyCycleInitialized(int32_t index);
+void HALSIM_SetDutyCycleInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterDutyCycleOutputCallback(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleOutputCallback(int32_t index, int32_t uid);
+double HALSIM_GetDutyCycleOutput(int32_t index);
+void HALSIM_SetDutyCycleOutput(int32_t index, double output);
+
+int32_t HALSIM_RegisterDutyCycleFrequencyCallback(int32_t index,
+                                                  HAL_NotifyCallback callback,
+                                                  void* param,
+                                                  HAL_Bool initialNotify);
+void HALSIM_CancelDutyCycleFrequencyCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetDutyCycleFrequency(int32_t index);
+void HALSIM_SetDutyCycleFrequency(int32_t index, int32_t frequency);
+
+void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/EncoderData.h

diff --git a/hal/src/main/native/include/hal/simulation/EncoderData.h b/hal/src/main/native/include/hal/simulation/EncoderData.h
new file mode 100644
index 0000000..80c142d
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/EncoderData.h
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int32_t HALSIM_FindEncoderForChannel(int32_t channel);
+
+void HALSIM_ResetEncoderData(int32_t index);
+int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index);
+int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index);
+int32_t HALSIM_RegisterEncoderInitializedCallback(int32_t index,
+                                                  HAL_NotifyCallback callback,
+                                                  void* param,
+                                                  HAL_Bool initialNotify);
+void HALSIM_CancelEncoderInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderInitialized(int32_t index);
+void HALSIM_SetEncoderInitialized(int32_t index, HAL_Bool initialized);
+
+HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index);
+
+int32_t HALSIM_RegisterEncoderCountCallback(int32_t index,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+void HALSIM_CancelEncoderCountCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetEncoderCount(int32_t index);
+void HALSIM_SetEncoderCount(int32_t index, int32_t count);
+
+int32_t HALSIM_RegisterEncoderPeriodCallback(int32_t index,
+                                             HAL_NotifyCallback callback,
+                                             void* param,
+                                             HAL_Bool initialNotify);
+void HALSIM_CancelEncoderPeriodCallback(int32_t index, int32_t uid);
+double HALSIM_GetEncoderPeriod(int32_t index);
+void HALSIM_SetEncoderPeriod(int32_t index, double period);
+
+int32_t HALSIM_RegisterEncoderResetCallback(int32_t index,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+void HALSIM_CancelEncoderResetCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderReset(int32_t index);
+void HALSIM_SetEncoderReset(int32_t index, HAL_Bool reset);
+
+int32_t HALSIM_RegisterEncoderMaxPeriodCallback(int32_t index,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify);
+void HALSIM_CancelEncoderMaxPeriodCallback(int32_t index, int32_t uid);
+double HALSIM_GetEncoderMaxPeriod(int32_t index);
+void HALSIM_SetEncoderMaxPeriod(int32_t index, double maxPeriod);
+
+int32_t HALSIM_RegisterEncoderDirectionCallback(int32_t index,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify);
+void HALSIM_CancelEncoderDirectionCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderDirection(int32_t index);
+void HALSIM_SetEncoderDirection(int32_t index, HAL_Bool direction);
+
+int32_t HALSIM_RegisterEncoderReverseDirectionCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelEncoderReverseDirectionCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetEncoderReverseDirection(int32_t index);
+void HALSIM_SetEncoderReverseDirection(int32_t index,
+                                       HAL_Bool reverseDirection);
+
+int32_t HALSIM_RegisterEncoderSamplesToAverageCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelEncoderSamplesToAverageCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetEncoderSamplesToAverage(int32_t index);
+void HALSIM_SetEncoderSamplesToAverage(int32_t index, int32_t samplesToAverage);
+
+int32_t HALSIM_RegisterEncoderDistancePerPulseCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelEncoderDistancePerPulseCallback(int32_t index, int32_t uid);
+double HALSIM_GetEncoderDistancePerPulse(int32_t index);
+void HALSIM_SetEncoderDistancePerPulse(int32_t index, double distancePerPulse);
+
+void HALSIM_RegisterEncoderAllCallbacks(int32_t index,
+                                        HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
+
+void HALSIM_SetEncoderDistance(int32_t index, double distance);
+double HALSIM_GetEncoderDistance(int32_t index);
+void HALSIM_SetEncoderRate(int32_t index, double rate);
+double HALSIM_GetEncoderRate(int32_t index);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/I2CData.h

diff --git a/hal/src/main/native/include/hal/simulation/I2CData.h b/hal/src/main/native/include/hal/simulation/I2CData.h
new file mode 100644
index 0000000..3d72850
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/I2CData.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetI2CData(int32_t index);
+
+int32_t HALSIM_RegisterI2CInitializedCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelI2CInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetI2CInitialized(int32_t index);
+void HALSIM_SetI2CInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterI2CReadCallback(int32_t index,
+                                       HAL_BufferCallback callback,
+                                       void* param);
+void HALSIM_CancelI2CReadCallback(int32_t index, int32_t uid);
+
+int32_t HALSIM_RegisterI2CWriteCallback(int32_t index,
+                                        HAL_ConstBufferCallback callback,
+                                        void* param);
+void HALSIM_CancelI2CWriteCallback(int32_t index, int32_t uid);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

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

diff --git a/hal/src/main/native/include/hal/simulation/MockHooks.h b/hal/src/main/native/include/hal/simulation/MockHooks.h
new file mode 100644
index 0000000..cef205e
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/MockHooks.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/HALBase.h"
+#include "hal/Types.h"
+
+extern "C" {
+void HALSIM_SetRuntimeType(HAL_RuntimeType type);
+void HALSIM_WaitForProgramStart(void);
+void HALSIM_SetProgramStarted(void);
+HAL_Bool HALSIM_GetProgramStarted(void);
+void HALSIM_RestartTiming(void);
+void HALSIM_PauseTiming(void);
+void HALSIM_ResumeTiming(void);
+HAL_Bool HALSIM_IsTimingPaused(void);
+void HALSIM_StepTiming(uint64_t delta);
+void HALSIM_StepTimingAsync(uint64_t delta);
+
+typedef int32_t (*HALSIM_SendErrorHandler)(
+    HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, const char* details,
+    const char* location, const char* callStack, HAL_Bool printMsg);
+void HALSIM_SetSendError(HALSIM_SendErrorHandler handler);
+
+typedef int32_t (*HALSIM_SendConsoleLineHandler)(const char* line);
+void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler);
+
+}  // extern "C"

hal/src/main/native/include/hal/simulation/NotifierData.h

diff --git a/hal/src/main/native/include/hal/simulation/NotifierData.h b/hal/src/main/native/include/hal/simulation/NotifierData.h
new file mode 100644
index 0000000..a5b68b6
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/NotifierData.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct HALSIM_NotifierInfo {
+  HAL_NotifierHandle handle;
+  char name[64];
+  uint64_t timeout;
+  HAL_Bool waitTimeValid;
+};
+
+uint64_t HALSIM_GetNextNotifierTimeout(void);
+
+int32_t HALSIM_GetNumNotifiers(void);
+
+/**
+ * Gets detailed information about each notifier.
+ *
+ * @param arr array of information to be filled
+ * @param size size of arr
+ * @return Number of notifiers; note: may be larger than passed-in size
+ */
+int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/NotifyListener.h

diff --git a/hal/src/main/native/include/hal/simulation/NotifyListener.h b/hal/src/main/native/include/hal/simulation/NotifyListener.h
new file mode 100644
index 0000000..a1430c4
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/NotifyListener.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Value.h"
+
+typedef void (*HAL_NotifyCallback)(const char* name, void* param,
+                                   const struct HAL_Value* value);
+
+typedef void (*HAL_BufferCallback)(const char* name, void* param,
+                                   unsigned char* buffer, unsigned int count);
+
+typedef void (*HAL_ConstBufferCallback)(const char* name, void* param,
+                                        const unsigned char* buffer,
+                                        unsigned int count);
+
+#ifdef __cplusplus
+
+namespace hal {
+
+template <typename CallbackFunction>
+struct HalCallbackListener {
+  HalCallbackListener() = default;
+  HalCallbackListener(void* param_, CallbackFunction callback_)
+      : callback(callback_), param(param_) {}
+
+  explicit operator bool() const { return callback != nullptr; }
+
+  CallbackFunction callback = nullptr;
+  void* param = nullptr;
+};
+
+}  // namespace hal
+
+#endif

hal/src/main/native/include/hal/simulation/PCMData.h

diff --git a/hal/src/main/native/include/hal/simulation/PCMData.h b/hal/src/main/native/include/hal/simulation/PCMData.h
new file mode 100644
index 0000000..ad2188d
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/PCMData.h
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetPCMData(int32_t index);
+int32_t HALSIM_RegisterPCMSolenoidInitializedCallback(
+    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelPCMSolenoidInitializedCallback(int32_t index, int32_t channel,
+                                                 int32_t uid);
+HAL_Bool HALSIM_GetPCMSolenoidInitialized(int32_t index, int32_t channel);
+void HALSIM_SetPCMSolenoidInitialized(int32_t index, int32_t channel,
+                                      HAL_Bool solenoidInitialized);
+
+int32_t HALSIM_RegisterPCMSolenoidOutputCallback(int32_t index, int32_t channel,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelPCMSolenoidOutputCallback(int32_t index, int32_t channel,
+                                            int32_t uid);
+HAL_Bool HALSIM_GetPCMSolenoidOutput(int32_t index, int32_t channel);
+void HALSIM_SetPCMSolenoidOutput(int32_t index, int32_t channel,
+                                 HAL_Bool solenoidOutput);
+
+int32_t HALSIM_RegisterPCMCompressorInitializedCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelPCMCompressorInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMCompressorInitialized(int32_t index);
+void HALSIM_SetPCMCompressorInitialized(int32_t index,
+                                        HAL_Bool compressorInitialized);
+
+int32_t HALSIM_RegisterPCMCompressorOnCallback(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+void HALSIM_CancelPCMCompressorOnCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMCompressorOn(int32_t index);
+void HALSIM_SetPCMCompressorOn(int32_t index, HAL_Bool compressorOn);
+
+int32_t HALSIM_RegisterPCMClosedLoopEnabledCallback(int32_t index,
+                                                    HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelPCMClosedLoopEnabledCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMClosedLoopEnabled(int32_t index);
+void HALSIM_SetPCMClosedLoopEnabled(int32_t index, HAL_Bool closedLoopEnabled);
+
+int32_t HALSIM_RegisterPCMPressureSwitchCallback(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelPCMPressureSwitchCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPCMPressureSwitch(int32_t index);
+void HALSIM_SetPCMPressureSwitch(int32_t index, HAL_Bool pressureSwitch);
+
+int32_t HALSIM_RegisterPCMCompressorCurrentCallback(int32_t index,
+                                                    HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelPCMCompressorCurrentCallback(int32_t index, int32_t uid);
+double HALSIM_GetPCMCompressorCurrent(int32_t index);
+void HALSIM_SetPCMCompressorCurrent(int32_t index, double compressorCurrent);
+
+void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values);
+void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values);
+
+void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
+                                               HAL_NotifyCallback callback,
+                                               void* param,
+                                               HAL_Bool initialNotify);
+
+void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/PDPData.h

diff --git a/hal/src/main/native/include/hal/simulation/PDPData.h b/hal/src/main/native/include/hal/simulation/PDPData.h
new file mode 100644
index 0000000..7ec11a4
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/PDPData.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetPDPData(int32_t index);
+int32_t HALSIM_RegisterPDPInitializedCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelPDPInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPDPInitialized(int32_t index);
+void HALSIM_SetPDPInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterPDPTemperatureCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelPDPTemperatureCallback(int32_t index, int32_t uid);
+double HALSIM_GetPDPTemperature(int32_t index);
+void HALSIM_SetPDPTemperature(int32_t index, double temperature);
+
+int32_t HALSIM_RegisterPDPVoltageCallback(int32_t index,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPDPVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetPDPVoltage(int32_t index);
+void HALSIM_SetPDPVoltage(int32_t index, double voltage);
+
+int32_t HALSIM_RegisterPDPCurrentCallback(int32_t index, int32_t channel,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPDPCurrentCallback(int32_t index, int32_t channel,
+                                     int32_t uid);
+double HALSIM_GetPDPCurrent(int32_t index, int32_t channel);
+void HALSIM_SetPDPCurrent(int32_t index, int32_t channel, double current);
+
+void HALSIM_GetPDPAllCurrents(int32_t index, double* currents);
+void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents);
+
+void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/PWMData.h

diff --git a/hal/src/main/native/include/hal/simulation/PWMData.h b/hal/src/main/native/include/hal/simulation/PWMData.h
new file mode 100644
index 0000000..a536710
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/PWMData.h
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetPWMData(int32_t index);
+int32_t HALSIM_RegisterPWMInitializedCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelPWMInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPWMInitialized(int32_t index);
+void HALSIM_SetPWMInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterPWMRawValueCallback(int32_t index,
+                                           HAL_NotifyCallback callback,
+                                           void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPWMRawValueCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetPWMRawValue(int32_t index);
+void HALSIM_SetPWMRawValue(int32_t index, int32_t rawValue);
+
+int32_t HALSIM_RegisterPWMSpeedCallback(int32_t index,
+                                        HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPWMSpeedCallback(int32_t index, int32_t uid);
+double HALSIM_GetPWMSpeed(int32_t index);
+void HALSIM_SetPWMSpeed(int32_t index, double speed);
+
+int32_t HALSIM_RegisterPWMPositionCallback(int32_t index,
+                                           HAL_NotifyCallback callback,
+                                           void* param, HAL_Bool initialNotify);
+void HALSIM_CancelPWMPositionCallback(int32_t index, int32_t uid);
+double HALSIM_GetPWMPosition(int32_t index);
+void HALSIM_SetPWMPosition(int32_t index, double position);
+
+int32_t HALSIM_RegisterPWMPeriodScaleCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelPWMPeriodScaleCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetPWMPeriodScale(int32_t index);
+void HALSIM_SetPWMPeriodScale(int32_t index, int32_t periodScale);
+
+int32_t HALSIM_RegisterPWMZeroLatchCallback(int32_t index,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+void HALSIM_CancelPWMZeroLatchCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetPWMZeroLatch(int32_t index);
+void HALSIM_SetPWMZeroLatch(int32_t index, HAL_Bool zeroLatch);
+
+void HALSIM_RegisterPWMAllCallbacks(int32_t index, HAL_NotifyCallback callback,
+                                    void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/RelayData.h

diff --git a/hal/src/main/native/include/hal/simulation/RelayData.h b/hal/src/main/native/include/hal/simulation/RelayData.h
new file mode 100644
index 0000000..1329d29
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/RelayData.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetRelayData(int32_t index);
+int32_t HALSIM_RegisterRelayInitializedForwardCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelRelayInitializedForwardCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayInitializedForward(int32_t index);
+void HALSIM_SetRelayInitializedForward(int32_t index,
+                                       HAL_Bool initializedForward);
+
+int32_t HALSIM_RegisterRelayInitializedReverseCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelRelayInitializedReverseCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayInitializedReverse(int32_t index);
+void HALSIM_SetRelayInitializedReverse(int32_t index,
+                                       HAL_Bool initializedReverse);
+
+int32_t HALSIM_RegisterRelayForwardCallback(int32_t index,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+void HALSIM_CancelRelayForwardCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayForward(int32_t index);
+void HALSIM_SetRelayForward(int32_t index, HAL_Bool forward);
+
+int32_t HALSIM_RegisterRelayReverseCallback(int32_t index,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify);
+void HALSIM_CancelRelayReverseCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetRelayReverse(int32_t index);
+void HALSIM_SetRelayReverse(int32_t index, HAL_Bool reverse);
+
+void HALSIM_RegisterRelayAllCallbacks(int32_t index,
+                                      HAL_NotifyCallback callback, void* param,
+                                      HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/RoboRioData.h

diff --git a/hal/src/main/native/include/hal/simulation/RoboRioData.h b/hal/src/main/native/include/hal/simulation/RoboRioData.h
new file mode 100644
index 0000000..49d32f3
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/RoboRioData.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetRoboRioData(void);
+int32_t HALSIM_RegisterRoboRioFPGAButtonCallback(HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioFPGAButtonCallback(int32_t uid);
+HAL_Bool HALSIM_GetRoboRioFPGAButton(void);
+void HALSIM_SetRoboRioFPGAButton(HAL_Bool fPGAButton);
+
+int32_t HALSIM_RegisterRoboRioVInVoltageCallback(HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioVInVoltageCallback(int32_t uid);
+double HALSIM_GetRoboRioVInVoltage(void);
+void HALSIM_SetRoboRioVInVoltage(double vInVoltage);
+
+int32_t HALSIM_RegisterRoboRioVInCurrentCallback(HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioVInCurrentCallback(int32_t uid);
+double HALSIM_GetRoboRioVInCurrent(void);
+void HALSIM_SetRoboRioVInCurrent(double vInCurrent);
+
+int32_t HALSIM_RegisterRoboRioUserVoltage6VCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserVoltage6VCallback(int32_t uid);
+double HALSIM_GetRoboRioUserVoltage6V(void);
+void HALSIM_SetRoboRioUserVoltage6V(double userVoltage6V);
+
+int32_t HALSIM_RegisterRoboRioUserCurrent6VCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserCurrent6VCallback(int32_t uid);
+double HALSIM_GetRoboRioUserCurrent6V(void);
+void HALSIM_SetRoboRioUserCurrent6V(double userCurrent6V);
+
+int32_t HALSIM_RegisterRoboRioUserActive6VCallback(HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserActive6VCallback(int32_t uid);
+HAL_Bool HALSIM_GetRoboRioUserActive6V(void);
+void HALSIM_SetRoboRioUserActive6V(HAL_Bool userActive6V);
+
+int32_t HALSIM_RegisterRoboRioUserVoltage5VCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserVoltage5VCallback(int32_t uid);
+double HALSIM_GetRoboRioUserVoltage5V(void);
+void HALSIM_SetRoboRioUserVoltage5V(double userVoltage5V);
+
+int32_t HALSIM_RegisterRoboRioUserCurrent5VCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserCurrent5VCallback(int32_t uid);
+double HALSIM_GetRoboRioUserCurrent5V(void);
+void HALSIM_SetRoboRioUserCurrent5V(double userCurrent5V);
+
+int32_t HALSIM_RegisterRoboRioUserActive5VCallback(HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserActive5VCallback(int32_t uid);
+HAL_Bool HALSIM_GetRoboRioUserActive5V(void);
+void HALSIM_SetRoboRioUserActive5V(HAL_Bool userActive5V);
+
+int32_t HALSIM_RegisterRoboRioUserVoltage3V3Callback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserVoltage3V3Callback(int32_t uid);
+double HALSIM_GetRoboRioUserVoltage3V3(void);
+void HALSIM_SetRoboRioUserVoltage3V3(double userVoltage3V3);
+
+int32_t HALSIM_RegisterRoboRioUserCurrent3V3Callback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserCurrent3V3Callback(int32_t uid);
+double HALSIM_GetRoboRioUserCurrent3V3(void);
+void HALSIM_SetRoboRioUserCurrent3V3(double userCurrent3V3);
+
+int32_t HALSIM_RegisterRoboRioUserActive3V3Callback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserActive3V3Callback(int32_t uid);
+HAL_Bool HALSIM_GetRoboRioUserActive3V3(void);
+void HALSIM_SetRoboRioUserActive3V3(HAL_Bool userActive3V3);
+
+int32_t HALSIM_RegisterRoboRioUserFaults6VCallback(HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserFaults6VCallback(int32_t uid);
+int32_t HALSIM_GetRoboRioUserFaults6V(void);
+void HALSIM_SetRoboRioUserFaults6V(int32_t userFaults6V);
+
+int32_t HALSIM_RegisterRoboRioUserFaults5VCallback(HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserFaults5VCallback(int32_t uid);
+int32_t HALSIM_GetRoboRioUserFaults5V(void);
+void HALSIM_SetRoboRioUserFaults5V(int32_t userFaults5V);
+
+int32_t HALSIM_RegisterRoboRioUserFaults3V3Callback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioUserFaults3V3Callback(int32_t uid);
+int32_t HALSIM_GetRoboRioUserFaults3V3(void);
+void HALSIM_SetRoboRioUserFaults3V3(int32_t userFaults3V3);
+
+void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h

diff --git a/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h b/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h
new file mode 100644
index 0000000..6515aa8
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetSPIAccelerometerData(int32_t index);
+int32_t HALSIM_RegisterSPIAccelerometerActiveCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerActiveCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetSPIAccelerometerActive(int32_t index);
+void HALSIM_SetSPIAccelerometerActive(int32_t index, HAL_Bool active);
+
+int32_t HALSIM_RegisterSPIAccelerometerRangeCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerRangeCallback(int32_t index, int32_t uid);
+int32_t HALSIM_GetSPIAccelerometerRange(int32_t index);
+void HALSIM_SetSPIAccelerometerRange(int32_t index, int32_t range);
+
+int32_t HALSIM_RegisterSPIAccelerometerXCallback(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerXCallback(int32_t index, int32_t uid);
+double HALSIM_GetSPIAccelerometerX(int32_t index);
+void HALSIM_SetSPIAccelerometerX(int32_t index, double x);
+
+int32_t HALSIM_RegisterSPIAccelerometerYCallback(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerYCallback(int32_t index, int32_t uid);
+double HALSIM_GetSPIAccelerometerY(int32_t index);
+void HALSIM_SetSPIAccelerometerY(int32_t index, double y);
+
+int32_t HALSIM_RegisterSPIAccelerometerZCallback(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelSPIAccelerometerZCallback(int32_t index, int32_t uid);
+double HALSIM_GetSPIAccelerometerZ(int32_t index);
+void HALSIM_SetSPIAccelerometerZ(int32_t index, double z);
+
+void HALSIM_RegisterSPIAccelerometerAllCallbcaks(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/SPIData.h

diff --git a/hal/src/main/native/include/hal/simulation/SPIData.h b/hal/src/main/native/include/hal/simulation/SPIData.h
new file mode 100644
index 0000000..149a1ef
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/SPIData.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+typedef void (*HAL_SpiReadAutoReceiveBufferCallback)(const char* name,
+                                                     void* param,
+                                                     uint32_t* buffer,
+                                                     int32_t numToRead,
+                                                     int32_t* outputCount);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetSPIData(int32_t index);
+
+int32_t HALSIM_RegisterSPIInitializedCallback(int32_t index,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+void HALSIM_CancelSPIInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetSPIInitialized(int32_t index);
+void HALSIM_SetSPIInitialized(int32_t index, HAL_Bool initialized);
+
+int32_t HALSIM_RegisterSPIReadCallback(int32_t index,
+                                       HAL_BufferCallback callback,
+                                       void* param);
+void HALSIM_CancelSPIReadCallback(int32_t index, int32_t uid);
+
+int32_t HALSIM_RegisterSPIWriteCallback(int32_t index,
+                                        HAL_ConstBufferCallback callback,
+                                        void* param);
+void HALSIM_CancelSPIWriteCallback(int32_t index, int32_t uid);
+
+int32_t HALSIM_RegisterSPIReadAutoReceivedDataCallback(
+    int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
+void HALSIM_CancelSPIReadAutoReceivedDataCallback(int32_t index, int32_t uid);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h

diff --git a/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h b/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h
new file mode 100644
index 0000000..31f64ef
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+
+#include <wpi/Compiler.h>
+#include <wpi/UidVector.h>
+#include <wpi/spinlock.h>
+
+#include "hal/simulation/NotifyListener.h"
+
+namespace hal {
+
+namespace impl {
+
+class SimCallbackRegistryBase {
+ public:
+  using RawFunctor = void (*)();
+
+ protected:
+  using CallbackVector = wpi::UidVector<HalCallbackListener<RawFunctor>, 4>;
+
+ public:
+  void Cancel(int32_t uid) {
+    std::scoped_lock lock(m_mutex);
+    if (m_callbacks && uid > 0) m_callbacks->erase(uid - 1);
+  }
+
+  void Reset() {
+    std::scoped_lock lock(m_mutex);
+    DoReset();
+  }
+
+  wpi::recursive_spinlock& GetMutex() { return m_mutex; }
+
+ protected:
+  int32_t DoRegister(RawFunctor callback, void* param) {
+    // Must return -1 on a null callback for error handling
+    if (callback == nullptr) return -1;
+    if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+    return m_callbacks->emplace_back(param, callback) + 1;
+  }
+
+  LLVM_ATTRIBUTE_ALWAYS_INLINE void DoReset() {
+    if (m_callbacks) m_callbacks->clear();
+  }
+
+  mutable wpi::recursive_spinlock m_mutex;
+  std::unique_ptr<CallbackVector> m_callbacks;
+};
+
+}  // namespace impl
+
+/**
+ * Simulation callback registry.  Provides callback functionality.
+ *
+ * @tparam CallbackFunction callback function type (e.g. HAL_BufferCallback)
+ * @tparam GetName function that returns a const char* for the name
+ */
+template <typename CallbackFunction, const char* (*GetName)()>
+class SimCallbackRegistry : public impl::SimCallbackRegistryBase {
+ public:
+  int32_t Register(CallbackFunction callback, void* param) {
+    std::scoped_lock lock(m_mutex);
+    return DoRegister(reinterpret_cast<RawFunctor>(callback), param);
+  }
+
+  template <typename... U>
+  void Invoke(U&&... u) const {
+#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
+    std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
+#else
+    std::scoped_lock lock(m_mutex);
+#endif
+    if (m_callbacks) {
+      const char* name = GetName();
+      for (auto&& cb : *m_callbacks)
+        reinterpret_cast<CallbackFunction>(cb.callback)(name, cb.param,
+                                                        std::forward<U>(u)...);
+    }
+  }
+
+  template <typename... U>
+  LLVM_ATTRIBUTE_ALWAYS_INLINE void operator()(U&&... u) const {
+    return Invoke(std::forward<U>(u)...);
+  }
+};
+
+/**
+ * Define a name functor for use with SimCallbackRegistry.
+ * This creates a function named GetNAMEName() that returns "NAME".
+ * @param NAME the name to return
+ */
+#define HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(NAME)           \
+  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr const char* \
+      Get##NAME##Name() {                                   \
+    return #NAME;                                           \
+  }
+
+/**
+ * Define a standard C API for SimCallbackRegistry.
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      int32_t index, TYPE callback, void* param)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
+ *
+ * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data array
+ * @param LOWERNAME the lowercase name of the backing data registry
+ */
+#define HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, NS, CAPINAME, DATA,     \
+                                            LOWERNAME)                    \
+  int32_t NS##_Register##CAPINAME##Callback(int32_t index, TYPE callback, \
+                                            void* param) {                \
+    return DATA[index].LOWERNAME.Register(callback, param);               \
+  }                                                                       \
+                                                                          \
+  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) {      \
+    DATA[index].LOWERNAME.Cancel(uid);                                    \
+  }
+
+/**
+ * Define a standard C API for SimCallbackRegistry (no index variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(TYPE callback, void* param)
+ * - void NS_CancelCAPINAMECallback(int32_t uid)
+ *
+ * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data pointer
+ * @param LOWERNAME the lowercase name of the backing data registry
+ */
+#define HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI_NOINDEX(TYPE, NS, CAPINAME, DATA, \
+                                                    LOWERNAME)                \
+  int32_t NS##_Register##CAPINAME##Callback(TYPE callback, void* param) {     \
+    return DATA->LOWERNAME.Register(callback, param);                         \
+  }                                                                           \
+                                                                              \
+  void NS##_Cancel##CAPINAME##Callback(int32_t uid) {                         \
+    DATA->LOWERNAME.Cancel(uid);                                              \
+  }
+
+/**
+ * Define a stub standard C API for SimCallbackRegistry.
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      int32_t index, TYPE callback, void* param)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
+ *
+ * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ */
+#define HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, NS, CAPINAME)             \
+  int32_t NS##_Register##CAPINAME##Callback(int32_t index, TYPE callback, \
+                                            void* param) {                \
+    return 0;                                                             \
+  }                                                                       \
+                                                                          \
+  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) {}
+
+/**
+ * Define a stub standard C API for SimCallbackRegistry (no index variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(TYPE callback, void* param)
+ * - void NS_CancelCAPINAMECallback(int32_t uid)
+ *
+ * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ */
+#define HAL_SIMCALLBACKREGISTRY_STUB_CAPI_NOINDEX(TYPE, NS, CAPINAME)     \
+  int32_t NS##_Register##CAPINAME##Callback(TYPE callback, void* param) { \
+    return 0;                                                             \
+  }                                                                       \
+                                                                          \
+  void NS##_Cancel##CAPINAME##Callback(int32_t uid) {}
+
+}  // namespace hal

hal/src/main/native/include/hal/simulation/SimDataValue.h

diff --git a/hal/src/main/native/include/hal/simulation/SimDataValue.h b/hal/src/main/native/include/hal/simulation/SimDataValue.h
new file mode 100644
index 0000000..ff333a5
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/SimDataValue.h
@@ -0,0 +1,313 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/Compiler.h>
+#include <wpi/UidVector.h>
+#include <wpi/spinlock.h>
+
+#include "hal/simulation/NotifyListener.h"
+#include "hal/simulation/SimCallbackRegistry.h"
+
+namespace hal {
+
+namespace impl {
+template <typename T, HAL_Value (*MakeValue)(T)>
+class SimDataValueBase : protected SimCallbackRegistryBase {
+ public:
+  explicit SimDataValueBase(T value) : m_value(value) {}
+
+  LLVM_ATTRIBUTE_ALWAYS_INLINE void CancelCallback(int32_t uid) { Cancel(uid); }
+
+  T Get() const {
+    std::scoped_lock lock(m_mutex);
+    return m_value;
+  }
+
+  LLVM_ATTRIBUTE_ALWAYS_INLINE operator T() const { return Get(); }
+
+  void Reset(T value) {
+    std::scoped_lock lock(m_mutex);
+    DoReset();
+    m_value = value;
+  }
+
+  wpi::recursive_spinlock& GetMutex() { return m_mutex; }
+
+ protected:
+  int32_t DoRegisterCallback(HAL_NotifyCallback callback, void* param,
+                             HAL_Bool initialNotify, const char* name) {
+    std::unique_lock lock(m_mutex);
+    int32_t newUid = DoRegister(reinterpret_cast<RawFunctor>(callback), param);
+    if (newUid == -1) return -1;
+    if (initialNotify) {
+      // We know that the callback is not null because of earlier null check
+      HAL_Value value = MakeValue(m_value);
+      lock.unlock();
+      callback(name, param, &value);
+    }
+    return newUid;
+  }
+
+  void DoSet(T value, const char* name) {
+    std::scoped_lock lock(this->m_mutex);
+    if (m_value != value) {
+      m_value = value;
+      if (m_callbacks) {
+        HAL_Value halValue = MakeValue(value);
+        for (auto&& cb : *m_callbacks)
+          reinterpret_cast<HAL_NotifyCallback>(cb.callback)(name, cb.param,
+                                                            &halValue);
+      }
+    }
+  }
+
+  T m_value;
+};
+}  // namespace impl
+
+/**
+ * Simulation data value wrapper.  Provides callback functionality when the
+ * data value is changed.
+ *
+ * @tparam T value type (e.g. double)
+ * @tparam MakeValue function that takes a T and returns a HAL_Value
+ * @tparam GetName function that returns a const char* for the name
+ * @tparam GetDefault function that returns the default T (used only for
+ *                    default constructor)
+ */
+template <typename T, HAL_Value (*MakeValue)(T), const char* (*GetName)(),
+          T (*GetDefault)() = nullptr>
+class SimDataValue final : public impl::SimDataValueBase<T, MakeValue> {
+ public:
+  SimDataValue()
+      : impl::SimDataValueBase<T, MakeValue>(
+            GetDefault != nullptr ? GetDefault() : T()) {}
+  explicit SimDataValue(T value)
+      : impl::SimDataValueBase<T, MakeValue>(value) {}
+
+  LLVM_ATTRIBUTE_ALWAYS_INLINE int32_t RegisterCallback(
+      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {
+    return this->DoRegisterCallback(callback, param, initialNotify, GetName());
+  }
+
+  LLVM_ATTRIBUTE_ALWAYS_INLINE void Set(T value) {
+    this->DoSet(value, GetName());
+  }
+
+  LLVM_ATTRIBUTE_ALWAYS_INLINE SimDataValue& operator=(T value) {
+    Set(value);
+    return *this;
+  }
+};
+
+/**
+ * Define a name functor for use with SimDataValue.
+ * This creates a function named GetNAMEName() that returns "NAME".
+ * @param NAME the name to return
+ */
+#define HAL_SIMDATAVALUE_DEFINE_NAME(NAME)                  \
+  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr const char* \
+      Get##NAME##Name() {                                   \
+    return #NAME;                                           \
+  }
+
+/**
+ * Define a standard C API for simulation data.
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      int32_t index, HAL_NotifyCallback callback, void* param,
+ *      HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
+ * - TYPE NS_GetCAPINAME(int32_t index)
+ * - void NS_SetCAPINAME(int32_t index, TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data array
+ * @param LOWERNAME the lowercase name of the backing data variable
+ */
+#define HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, NS, CAPINAME, DATA, LOWERNAME)  \
+  int32_t NS##_Register##CAPINAME##Callback(                               \
+      int32_t index, HAL_NotifyCallback callback, void* param,             \
+      HAL_Bool initialNotify) {                                            \
+    return DATA[index].LOWERNAME.RegisterCallback(callback, param,         \
+                                                  initialNotify);          \
+  }                                                                        \
+                                                                           \
+  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) {       \
+    DATA[index].LOWERNAME.CancelCallback(uid);                             \
+  }                                                                        \
+                                                                           \
+  TYPE NS##_Get##CAPINAME(int32_t index) { return DATA[index].LOWERNAME; } \
+                                                                           \
+  void NS##_Set##CAPINAME(int32_t index, TYPE LOWERNAME) {                 \
+    DATA[index].LOWERNAME = LOWERNAME;                                     \
+  }
+
+/**
+ * Define a standard C API for simulation data (channel variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      int32_t index, int32_t channel, HAL_NotifyCallback callback,
+ *      void* param, HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t channel, int32_t uid)
+ * - TYPE NS_GetCAPINAME(int32_t index, int32_t channel)
+ * - void NS_SetCAPINAME(int32_t index, int32_t channel, TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data array
+ * @param LOWERNAME the lowercase name of the backing data variable array
+ */
+#define HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(TYPE, NS, CAPINAME, DATA,      \
+                                             LOWERNAME)                     \
+  int32_t NS##_Register##CAPINAME##Callback(                                \
+      int32_t index, int32_t channel, HAL_NotifyCallback callback,          \
+      void* param, HAL_Bool initialNotify) {                                \
+    return DATA[index].LOWERNAME[channel].RegisterCallback(callback, param, \
+                                                           initialNotify);  \
+  }                                                                         \
+                                                                            \
+  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t channel,      \
+                                       int32_t uid) {                       \
+    DATA[index].LOWERNAME[channel].CancelCallback(uid);                     \
+  }                                                                         \
+                                                                            \
+  TYPE NS##_Get##CAPINAME(int32_t index, int32_t channel) {                 \
+    return DATA[index].LOWERNAME[channel];                                  \
+  }                                                                         \
+                                                                            \
+  void NS##_Set##CAPINAME(int32_t index, int32_t channel, TYPE LOWERNAME) { \
+    DATA[index].LOWERNAME[channel] = LOWERNAME;                             \
+  }
+
+/**
+ * Define a standard C API for simulation data (no index variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t uid)
+ * - TYPE NS_GetCAPINAME(void)
+ * - void NS_SetCAPINAME(TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param DATA the backing data pointer
+ * @param LOWERNAME the lowercase name of the backing data variable
+ */
+#define HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, NS, CAPINAME, DATA,       \
+                                             LOWERNAME)                      \
+  int32_t NS##_Register##CAPINAME##Callback(                                 \
+      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {    \
+    return DATA->LOWERNAME.RegisterCallback(callback, param, initialNotify); \
+  }                                                                          \
+                                                                             \
+  void NS##_Cancel##CAPINAME##Callback(int32_t uid) {                        \
+    DATA->LOWERNAME.CancelCallback(uid);                                     \
+  }                                                                          \
+                                                                             \
+  TYPE NS##_Get##CAPINAME(void) { return DATA->LOWERNAME; }                  \
+                                                                             \
+  void NS##_Set##CAPINAME(TYPE LOWERNAME) { DATA->LOWERNAME = LOWERNAME; }
+
+/**
+ * Define a stub standard C API for simulation data.
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      int32_t index, HAL_NotifyCallback callback, void* param,
+ *      HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
+ * - TYPE NS_GetCAPINAME(int32_t index)
+ * - void NS_SetCAPINAME(int32_t index, TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param RETURN what to return from the Get function
+ */
+#define HAL_SIMDATAVALUE_STUB_CAPI(TYPE, NS, CAPINAME, RETURN)        \
+  int32_t NS##_Register##CAPINAME##Callback(                          \
+      int32_t index, HAL_NotifyCallback callback, void* param,        \
+      HAL_Bool initialNotify) {                                       \
+    return 0;                                                         \
+  }                                                                   \
+                                                                      \
+  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) {} \
+                                                                      \
+  TYPE NS##_Get##CAPINAME(int32_t index) { return RETURN; }           \
+                                                                      \
+  void NS##_Set##CAPINAME(int32_t index, TYPE) {}
+
+/**
+ * Define a stub standard C API for simulation data (channel variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      int32_t index, int32_t channel, HAL_NotifyCallback callback,
+ *      void* param, HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t index, int32_t channel, int32_t uid)
+ * - TYPE NS_GetCAPINAME(int32_t index, int32_t channel)
+ * - void NS_SetCAPINAME(int32_t index, int32_t channel, TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param RETURN what to return from the Get function
+ */
+#define HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(TYPE, NS, CAPINAME, RETURN)       \
+  int32_t NS##_Register##CAPINAME##Callback(                                 \
+      int32_t index, int32_t channel, HAL_NotifyCallback callback,           \
+      void* param, HAL_Bool initialNotify) {                                 \
+    return 0;                                                                \
+  }                                                                          \
+                                                                             \
+  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t channel,       \
+                                       int32_t uid) {}                       \
+                                                                             \
+  TYPE NS##_Get##CAPINAME(int32_t index, int32_t channel) { return RETURN; } \
+                                                                             \
+  void NS##_Set##CAPINAME(int32_t index, int32_t channel, TYPE) {}
+
+/**
+ * Define a stub standard C API for simulation data (no index variant).
+ *
+ * Functions defined:
+ * - int32 NS_RegisterCAPINAMECallback(
+ *      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify)
+ * - void NS_CancelCAPINAMECallback(int32_t uid)
+ * - TYPE NS_GetCAPINAME(void)
+ * - void NS_SetCAPINAME(TYPE value)
+ *
+ * @param TYPE the underlying value type (e.g. double)
+ * @param NS the "namespace" (e.g. HALSIM)
+ * @param CAPINAME the C API name (usually first letter capitalized)
+ * @param RETURN what to return from the Get function
+ */
+#define HAL_SIMDATAVALUE_STUB_CAPI_NOINDEX(TYPE, NS, CAPINAME, RETURN)    \
+  int32_t NS##_Register##CAPINAME##Callback(                              \
+      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) { \
+    return 0;                                                             \
+  }                                                                       \
+                                                                          \
+  void NS##_Cancel##CAPINAME##Callback(int32_t uid) {}                    \
+                                                                          \
+  TYPE NS##_Get##CAPINAME(void) { return RETURN; }                        \
+                                                                          \
+  void NS##_Set##CAPINAME(TYPE) {}
+
+}  // namespace hal

hal/src/main/native/include/hal/simulation/SimDeviceData.h

diff --git a/hal/src/main/native/include/hal/simulation/SimDeviceData.h b/hal/src/main/native/include/hal/simulation/SimDeviceData.h
new file mode 100644
index 0000000..e702c5c
--- /dev/null
+++ b/hal/src/main/native/include/hal/simulation/SimDeviceData.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/Value.h"
+
+typedef void (*HALSIM_SimDeviceCallback)(const char* name, void* param,
+                                         HAL_SimDeviceHandle handle);
+
+typedef void (*HALSIM_SimValueCallback)(const char* name, void* param,
+                                        HAL_SimValueHandle handle,
+                                        HAL_Bool readonly,
+                                        const struct HAL_Value* value);
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_SetSimDeviceEnabled(const char* prefix, HAL_Bool enabled);
+HAL_Bool HALSIM_IsSimDeviceEnabled(const char* name);
+
+int32_t HALSIM_RegisterSimDeviceCreatedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
+    HAL_Bool initialNotify);
+
+void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid);
+
+int32_t HALSIM_RegisterSimDeviceFreedCallback(
+    const char* prefix, void* param, HALSIM_SimDeviceCallback callback);
+
+void HALSIM_CancelSimDeviceFreedCallback(int32_t uid);
+
+HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name);
+
+const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle);
+
+HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle);
+
+void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
+                                HALSIM_SimDeviceCallback callback);
+
+int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
+                                               void* param,
+                                               HALSIM_SimValueCallback callback,
+                                               HAL_Bool initialNotify);
+
+void HALSIM_CancelSimValueCreatedCallback(int32_t uid);
+
+int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
+                                               void* param,
+                                               HALSIM_SimValueCallback callback,
+                                               HAL_Bool initialNotify);
+
+void HALSIM_CancelSimValueChangedCallback(int32_t uid);
+
+HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
+                                            const char* name);
+
+void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
+                               HALSIM_SimValueCallback callback);
+
+const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
+                                           int32_t* numOptions);
+
+void HALSIM_ResetSimDeviceData(void);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

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

diff --git a/hal/src/main/native/include/mockdata/AccelerometerData.h b/hal/src/main/native/include/mockdata/AccelerometerData.h
deleted file mode 100644
index aa89f6e..0000000
--- a/hal/src/main/native/include/mockdata/AccelerometerData.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Accelerometer.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetAccelerometerData(int32_t index);
-int32_t HALSIM_RegisterAccelerometerActiveCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelAccelerometerActiveCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetAccelerometerActive(int32_t index);
-void HALSIM_SetAccelerometerActive(int32_t index, HAL_Bool active);
-
-int32_t HALSIM_RegisterAccelerometerRangeCallback(int32_t index,
-                                                  HAL_NotifyCallback callback,
-                                                  void* param,
-                                                  HAL_Bool initialNotify);
-void HALSIM_CancelAccelerometerRangeCallback(int32_t index, int32_t uid);
-HAL_AccelerometerRange HALSIM_GetAccelerometerRange(int32_t index);
-void HALSIM_SetAccelerometerRange(int32_t index, HAL_AccelerometerRange range);
-
-int32_t HALSIM_RegisterAccelerometerXCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelAccelerometerXCallback(int32_t index, int32_t uid);
-double HALSIM_GetAccelerometerX(int32_t index);
-void HALSIM_SetAccelerometerX(int32_t index, double x);
-
-int32_t HALSIM_RegisterAccelerometerYCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelAccelerometerYCallback(int32_t index, int32_t uid);
-double HALSIM_GetAccelerometerY(int32_t index);
-void HALSIM_SetAccelerometerY(int32_t index, double y);
-
-int32_t HALSIM_RegisterAccelerometerZCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelAccelerometerZCallback(int32_t index, int32_t uid);
-double HALSIM_GetAccelerometerZ(int32_t index);
-void HALSIM_SetAccelerometerZ(int32_t index, double z);
-
-void HALSIM_RegisterAccelerometerAllCallbacks(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/AddressableLEDData.h b/hal/src/main/native/include/mockdata/AddressableLEDData.h
deleted file mode 100644
index 0d8f3f3..0000000
--- a/hal/src/main/native/include/mockdata/AddressableLEDData.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/AddressableLEDTypes.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetAddressableLEDData(int32_t index);
-
-int32_t HALSIM_RegisterAddressableLEDInitializedCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAddressableLEDInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetAddressableLEDInitialized(int32_t index);
-void HALSIM_SetAddressableLEDInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterAddressableLEDOutputPortCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAddressableLEDOutputPortCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetAddressableLEDOutputPort(int32_t index);
-void HALSIM_SetAddressableLEDOutputPort(int32_t index, int32_t outputPort);
-
-int32_t HALSIM_RegisterAddressableLEDLengthCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelAddressableLEDLengthCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetAddressableLEDLength(int32_t index);
-void HALSIM_SetAddressableLEDLength(int32_t index, int32_t length);
-
-int32_t HALSIM_RegisterAddressableLEDRunningCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAddressableLEDRunningCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetAddressableLEDRunning(int32_t index);
-void HALSIM_SetAddressableLEDRunning(int32_t index, HAL_Bool running);
-
-int32_t HALSIM_RegisterAddressableLEDDataCallback(
-    int32_t index, HAL_ConstBufferCallback callback, void* param);
-void HALSIM_CancelAddressableLEDDataCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetAddressableLEDData(int32_t index,
-                                     struct HAL_AddressableLEDData* data);
-void HALSIM_SetAddressableLEDData(int32_t index,
-                                  const struct HAL_AddressableLEDData* data,
-                                  int32_t length);
-
-void HALSIM_RegisterAddressableLEDAllCallbacks(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/AnalogGyroData.h b/hal/src/main/native/include/mockdata/AnalogGyroData.h
deleted file mode 100644
index 56c739d..0000000
--- a/hal/src/main/native/include/mockdata/AnalogGyroData.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetAnalogGyroData(int32_t index);
-int32_t HALSIM_RegisterAnalogGyroAngleCallback(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-void HALSIM_CancelAnalogGyroAngleCallback(int32_t index, int32_t uid);
-double HALSIM_GetAnalogGyroAngle(int32_t index);
-void HALSIM_SetAnalogGyroAngle(int32_t index, double angle);
-
-int32_t HALSIM_RegisterAnalogGyroRateCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelAnalogGyroRateCallback(int32_t index, int32_t uid);
-double HALSIM_GetAnalogGyroRate(int32_t index);
-void HALSIM_SetAnalogGyroRate(int32_t index, double rate);
-
-int32_t HALSIM_RegisterAnalogGyroInitializedCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogGyroInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetAnalogGyroInitialized(int32_t index);
-void HALSIM_SetAnalogGyroInitialized(int32_t index, HAL_Bool initialized);
-
-void HALSIM_RegisterAnalogGyroAllCallbacks(int32_t index,
-                                           HAL_NotifyCallback callback,
-                                           void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/AnalogInData.h b/hal/src/main/native/include/mockdata/AnalogInData.h
deleted file mode 100644
index e571563..0000000
--- a/hal/src/main/native/include/mockdata/AnalogInData.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetAnalogInData(int32_t index);
-int32_t HALSIM_RegisterAnalogInInitializedCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetAnalogInInitialized(int32_t index);
-void HALSIM_SetAnalogInInitialized(int32_t index, HAL_Bool initialized);
-
-HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index);
-
-int32_t HALSIM_RegisterAnalogInAverageBitsCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInAverageBitsCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetAnalogInAverageBits(int32_t index);
-void HALSIM_SetAnalogInAverageBits(int32_t index, int32_t averageBits);
-
-int32_t HALSIM_RegisterAnalogInOversampleBitsCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInOversampleBitsCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetAnalogInOversampleBits(int32_t index);
-void HALSIM_SetAnalogInOversampleBits(int32_t index, int32_t oversampleBits);
-
-int32_t HALSIM_RegisterAnalogInVoltageCallback(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInVoltageCallback(int32_t index, int32_t uid);
-double HALSIM_GetAnalogInVoltage(int32_t index);
-void HALSIM_SetAnalogInVoltage(int32_t index, double voltage);
-
-int32_t HALSIM_RegisterAnalogInAccumulatorInitializedCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInAccumulatorInitializedCallback(int32_t index,
-                                                         int32_t uid);
-HAL_Bool HALSIM_GetAnalogInAccumulatorInitialized(int32_t index);
-void HALSIM_SetAnalogInAccumulatorInitialized(int32_t index,
-                                              HAL_Bool accumulatorInitialized);
-
-int32_t HALSIM_RegisterAnalogInAccumulatorValueCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInAccumulatorValueCallback(int32_t index, int32_t uid);
-int64_t HALSIM_GetAnalogInAccumulatorValue(int32_t index);
-void HALSIM_SetAnalogInAccumulatorValue(int32_t index,
-                                        int64_t accumulatorValue);
-
-int32_t HALSIM_RegisterAnalogInAccumulatorCountCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInAccumulatorCountCallback(int32_t index, int32_t uid);
-int64_t HALSIM_GetAnalogInAccumulatorCount(int32_t index);
-void HALSIM_SetAnalogInAccumulatorCount(int32_t index,
-                                        int64_t accumulatorCount);
-
-int32_t HALSIM_RegisterAnalogInAccumulatorCenterCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInAccumulatorCenterCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetAnalogInAccumulatorCenter(int32_t index);
-void HALSIM_SetAnalogInAccumulatorCenter(int32_t index,
-                                         int32_t accumulatorCenter);
-
-int32_t HALSIM_RegisterAnalogInAccumulatorDeadbandCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogInAccumulatorDeadbandCallback(int32_t index,
-                                                      int32_t uid);
-int32_t HALSIM_GetAnalogInAccumulatorDeadband(int32_t index);
-void HALSIM_SetAnalogInAccumulatorDeadband(int32_t index,
-                                           int32_t accumulatorDeadband);
-
-void HALSIM_RegisterAnalogInAllCallbacks(int32_t index,
-                                         HAL_NotifyCallback callback,
-                                         void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/AnalogOutData.h b/hal/src/main/native/include/mockdata/AnalogOutData.h
deleted file mode 100644
index e8f55f6..0000000
--- a/hal/src/main/native/include/mockdata/AnalogOutData.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetAnalogOutData(int32_t index);
-int32_t HALSIM_RegisterAnalogOutVoltageCallback(int32_t index,
-                                                HAL_NotifyCallback callback,
-                                                void* param,
-                                                HAL_Bool initialNotify);
-void HALSIM_CancelAnalogOutVoltageCallback(int32_t index, int32_t uid);
-double HALSIM_GetAnalogOutVoltage(int32_t index);
-void HALSIM_SetAnalogOutVoltage(int32_t index, double voltage);
-
-int32_t HALSIM_RegisterAnalogOutInitializedCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogOutInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetAnalogOutInitialized(int32_t index);
-void HALSIM_SetAnalogOutInitialized(int32_t index, HAL_Bool initialized);
-
-void HALSIM_RegisterAnalogOutAllCallbacks(int32_t index,
-                                          HAL_NotifyCallback callback,
-                                          void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/AnalogTriggerData.h b/hal/src/main/native/include/mockdata/AnalogTriggerData.h
deleted file mode 100644
index 566f2f5..0000000
--- a/hal/src/main/native/include/mockdata/AnalogTriggerData.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-enum HALSIM_AnalogTriggerMode : int32_t {
-  HALSIM_AnalogTriggerUnassigned,
-  HALSIM_AnalogTriggerFiltered,
-  HALSIM_AnalogTriggerDutyCycle,
-  HALSIM_AnalogTriggerAveraged
-};
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetAnalogTriggerData(int32_t index);
-int32_t HALSIM_RegisterAnalogTriggerInitializedCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogTriggerInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetAnalogTriggerInitialized(int32_t index);
-void HALSIM_SetAnalogTriggerInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback(int32_t index,
-                                                         int32_t uid);
-double HALSIM_GetAnalogTriggerTriggerLowerBound(int32_t index);
-void HALSIM_SetAnalogTriggerTriggerLowerBound(int32_t index,
-                                              double triggerLowerBound);
-
-int32_t HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback(int32_t index,
-                                                         int32_t uid);
-double HALSIM_GetAnalogTriggerTriggerUpperBound(int32_t index);
-void HALSIM_SetAnalogTriggerTriggerUpperBound(int32_t index,
-                                              double triggerUpperBound);
-
-int32_t HALSIM_RegisterAnalogTriggerTriggerModeCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelAnalogTriggerTriggerModeCallback(int32_t index, int32_t uid);
-HALSIM_AnalogTriggerMode HALSIM_GetAnalogTriggerTriggerMode(int32_t index);
-void HALSIM_SetAnalogTriggerTriggerMode(int32_t index,
-                                        HALSIM_AnalogTriggerMode triggerMode);
-
-void HALSIM_RegisterAnalogTriggerAllCallbacks(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/CanData.h b/hal/src/main/native/include/mockdata/CanData.h
deleted file mode 100644
index 0d48290..0000000
--- a/hal/src/main/native/include/mockdata/CanData.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-
-typedef void (*HAL_CAN_SendMessageCallback)(const char* name, void* param,
-                                            uint32_t messageID,
-                                            const uint8_t* data,
-                                            uint8_t dataSize, int32_t periodMs,
-                                            int32_t* status);
-
-typedef void (*HAL_CAN_ReceiveMessageCallback)(
-    const char* name, void* param, uint32_t* messageID, uint32_t messageIDMask,
-    uint8_t* data, uint8_t* dataSize, uint32_t* timeStamp, int32_t* status);
-
-typedef void (*HAL_CAN_OpenStreamSessionCallback)(
-    const char* name, void* param, uint32_t* sessionHandle, uint32_t messageID,
-    uint32_t messageIDMask, uint32_t maxMessages, int32_t* status);
-
-typedef void (*HAL_CAN_CloseStreamSessionCallback)(const char* name,
-                                                   void* param,
-                                                   uint32_t sessionHandle);
-
-typedef void (*HAL_CAN_ReadStreamSessionCallback)(
-    const char* name, void* param, uint32_t sessionHandle,
-    struct HAL_CANStreamMessage* messages, uint32_t messagesToRead,
-    uint32_t* messagesRead, int32_t* status);
-
-typedef void (*HAL_CAN_GetCANStatusCallback)(
-    const char* name, void* param, float* percentBusUtilization,
-    uint32_t* busOffCount, uint32_t* txFullCount, uint32_t* receiveErrorCount,
-    uint32_t* transmitErrorCount, int32_t* status);
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetCanData(void);
-
-int32_t HALSIM_RegisterCanSendMessageCallback(
-    HAL_CAN_SendMessageCallback callback, void* param);
-void HALSIM_CancelCanSendMessageCallback(int32_t uid);
-
-int32_t HALSIM_RegisterCanReceiveMessageCallback(
-    HAL_CAN_ReceiveMessageCallback callback, void* param);
-void HALSIM_CancelCanReceiveMessageCallback(int32_t uid);
-
-int32_t HALSIM_RegisterCanOpenStreamCallback(
-    HAL_CAN_OpenStreamSessionCallback callback, void* param);
-void HALSIM_CancelCanOpenStreamCallback(int32_t uid);
-
-int32_t HALSIM_RegisterCanCloseStreamCallback(
-    HAL_CAN_CloseStreamSessionCallback callback, void* param);
-void HALSIM_CancelCanCloseStreamCallback(int32_t uid);
-
-int32_t HALSIM_RegisterCanReadStreamCallback(
-    HAL_CAN_ReadStreamSessionCallback callback, void* param);
-void HALSIM_CancelCanReadStreamCallback(int32_t uid);
-
-int32_t HALSIM_RegisterCanGetCANStatusCallback(
-    HAL_CAN_GetCANStatusCallback callback, void* param);
-void HALSIM_CancelCanGetCANStatusCallback(int32_t uid);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/DIOData.h b/hal/src/main/native/include/mockdata/DIOData.h
deleted file mode 100644
index d13eee1..0000000
--- a/hal/src/main/native/include/mockdata/DIOData.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetDIOData(int32_t index);
-int32_t HALSIM_RegisterDIOInitializedCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelDIOInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetDIOInitialized(int32_t index);
-void HALSIM_SetDIOInitialized(int32_t index, HAL_Bool initialized);
-
-HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index);
-
-int32_t HALSIM_RegisterDIOValueCallback(int32_t index,
-                                        HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-void HALSIM_CancelDIOValueCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetDIOValue(int32_t index);
-void HALSIM_SetDIOValue(int32_t index, HAL_Bool value);
-
-int32_t HALSIM_RegisterDIOPulseLengthCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelDIOPulseLengthCallback(int32_t index, int32_t uid);
-double HALSIM_GetDIOPulseLength(int32_t index);
-void HALSIM_SetDIOPulseLength(int32_t index, double pulseLength);
-
-int32_t HALSIM_RegisterDIOIsInputCallback(int32_t index,
-                                          HAL_NotifyCallback callback,
-                                          void* param, HAL_Bool initialNotify);
-void HALSIM_CancelDIOIsInputCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetDIOIsInput(int32_t index);
-void HALSIM_SetDIOIsInput(int32_t index, HAL_Bool isInput);
-
-int32_t HALSIM_RegisterDIOFilterIndexCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelDIOFilterIndexCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetDIOFilterIndex(int32_t index);
-void HALSIM_SetDIOFilterIndex(int32_t index, int32_t filterIndex);
-
-void HALSIM_RegisterDIOAllCallbacks(int32_t index, HAL_NotifyCallback callback,
-                                    void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/DigitalPWMData.h b/hal/src/main/native/include/mockdata/DigitalPWMData.h
deleted file mode 100644
index 5af17e3..0000000
--- a/hal/src/main/native/include/mockdata/DigitalPWMData.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetDigitalPWMData(int32_t index);
-int32_t HALSIM_RegisterDigitalPWMInitializedCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelDigitalPWMInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetDigitalPWMInitialized(int32_t index);
-void HALSIM_SetDigitalPWMInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterDigitalPWMDutyCycleCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelDigitalPWMDutyCycleCallback(int32_t index, int32_t uid);
-double HALSIM_GetDigitalPWMDutyCycle(int32_t index);
-void HALSIM_SetDigitalPWMDutyCycle(int32_t index, double dutyCycle);
-
-int32_t HALSIM_RegisterDigitalPWMPinCallback(int32_t index,
-                                             HAL_NotifyCallback callback,
-                                             void* param,
-                                             HAL_Bool initialNotify);
-void HALSIM_CancelDigitalPWMPinCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetDigitalPWMPin(int32_t index);
-void HALSIM_SetDigitalPWMPin(int32_t index, int32_t pin);
-
-void HALSIM_RegisterDigitalPWMAllCallbacks(int32_t index,
-                                           HAL_NotifyCallback callback,
-                                           void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/DriverStationData.h b/hal/src/main/native/include/mockdata/DriverStationData.h
deleted file mode 100644
index 1f69664..0000000
--- a/hal/src/main/native/include/mockdata/DriverStationData.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/DriverStationTypes.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetDriverStationData(void);
-int32_t HALSIM_RegisterDriverStationEnabledCallback(HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationEnabledCallback(int32_t uid);
-HAL_Bool HALSIM_GetDriverStationEnabled(void);
-void HALSIM_SetDriverStationEnabled(HAL_Bool enabled);
-
-int32_t HALSIM_RegisterDriverStationAutonomousCallback(
-    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationAutonomousCallback(int32_t uid);
-HAL_Bool HALSIM_GetDriverStationAutonomous(void);
-void HALSIM_SetDriverStationAutonomous(HAL_Bool autonomous);
-
-int32_t HALSIM_RegisterDriverStationTestCallback(HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationTestCallback(int32_t uid);
-HAL_Bool HALSIM_GetDriverStationTest(void);
-void HALSIM_SetDriverStationTest(HAL_Bool test);
-
-int32_t HALSIM_RegisterDriverStationEStopCallback(HAL_NotifyCallback callback,
-                                                  void* param,
-                                                  HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationEStopCallback(int32_t uid);
-HAL_Bool HALSIM_GetDriverStationEStop(void);
-void HALSIM_SetDriverStationEStop(HAL_Bool eStop);
-
-int32_t HALSIM_RegisterDriverStationFmsAttachedCallback(
-    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationFmsAttachedCallback(int32_t uid);
-HAL_Bool HALSIM_GetDriverStationFmsAttached(void);
-void HALSIM_SetDriverStationFmsAttached(HAL_Bool fmsAttached);
-
-int32_t HALSIM_RegisterDriverStationDsAttachedCallback(
-    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationDsAttachedCallback(int32_t uid);
-HAL_Bool HALSIM_GetDriverStationDsAttached(void);
-void HALSIM_SetDriverStationDsAttached(HAL_Bool dsAttached);
-
-int32_t HALSIM_RegisterDriverStationAllianceStationIdCallback(
-    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationAllianceStationIdCallback(int32_t uid);
-HAL_AllianceStationID HALSIM_GetDriverStationAllianceStationId(void);
-void HALSIM_SetDriverStationAllianceStationId(
-    HAL_AllianceStationID allianceStationId);
-
-int32_t HALSIM_RegisterDriverStationMatchTimeCallback(
-    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
-void HALSIM_CancelDriverStationMatchTimeCallback(int32_t uid);
-double HALSIM_GetDriverStationMatchTime(void);
-void HALSIM_SetDriverStationMatchTime(double matchTime);
-
-void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes);
-void HALSIM_SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs);
-void HALSIM_SetJoystickButtons(int32_t joystickNum,
-                               const HAL_JoystickButtons* buttons);
-void HALSIM_SetJoystickDescriptor(int32_t joystickNum,
-                                  const HAL_JoystickDescriptor* descriptor);
-
-void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
-                               int32_t* leftRumble, int32_t* rightRumble);
-
-void HALSIM_SetMatchInfo(const HAL_MatchInfo* info);
-
-void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-
-void HALSIM_NotifyDriverStationNewData(void);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/DutyCycleData.h b/hal/src/main/native/include/mockdata/DutyCycleData.h
deleted file mode 100644
index b97b355..0000000
--- a/hal/src/main/native/include/mockdata/DutyCycleData.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetDutyCycleData(int32_t index);
-int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index);
-
-int32_t HALSIM_RegisterDutyCycleInitializedCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelDutyCycleInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetDutyCycleInitialized(int32_t index);
-void HALSIM_SetDutyCycleInitialized(int32_t index, HAL_Bool initialized);
-
-HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index);
-
-int32_t HALSIM_RegisterDutyCycleOutputCallback(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-void HALSIM_CancelDutyCycleOutputCallback(int32_t index, int32_t uid);
-double HALSIM_GetDutyCycleOutput(int32_t index);
-void HALSIM_SetDutyCycleOutput(int32_t index, double output);
-
-int32_t HALSIM_RegisterDutyCycleFrequencyCallback(int32_t index,
-                                                  HAL_NotifyCallback callback,
-                                                  void* param,
-                                                  HAL_Bool initialNotify);
-void HALSIM_CancelDutyCycleFrequencyCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetDutyCycleFrequency(int32_t index);
-void HALSIM_SetDutyCycleFrequency(int32_t index, int32_t frequency);
-
-void HALSIM_RegisterDutyCycleAllCallbacks(int32_t index,
-                                          HAL_NotifyCallback callback,
-                                          void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/EncoderData.h b/hal/src/main/native/include/mockdata/EncoderData.h
deleted file mode 100644
index d5d12be..0000000
--- a/hal/src/main/native/include/mockdata/EncoderData.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetEncoderData(int32_t index);
-int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index);
-int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index);
-int32_t HALSIM_RegisterEncoderInitializedCallback(int32_t index,
-                                                  HAL_NotifyCallback callback,
-                                                  void* param,
-                                                  HAL_Bool initialNotify);
-void HALSIM_CancelEncoderInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetEncoderInitialized(int32_t index);
-void HALSIM_SetEncoderInitialized(int32_t index, HAL_Bool initialized);
-
-HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index);
-
-int32_t HALSIM_RegisterEncoderCountCallback(int32_t index,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify);
-void HALSIM_CancelEncoderCountCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetEncoderCount(int32_t index);
-void HALSIM_SetEncoderCount(int32_t index, int32_t count);
-
-int32_t HALSIM_RegisterEncoderPeriodCallback(int32_t index,
-                                             HAL_NotifyCallback callback,
-                                             void* param,
-                                             HAL_Bool initialNotify);
-void HALSIM_CancelEncoderPeriodCallback(int32_t index, int32_t uid);
-double HALSIM_GetEncoderPeriod(int32_t index);
-void HALSIM_SetEncoderPeriod(int32_t index, double period);
-
-int32_t HALSIM_RegisterEncoderResetCallback(int32_t index,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify);
-void HALSIM_CancelEncoderResetCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetEncoderReset(int32_t index);
-void HALSIM_SetEncoderReset(int32_t index, HAL_Bool reset);
-
-int32_t HALSIM_RegisterEncoderMaxPeriodCallback(int32_t index,
-                                                HAL_NotifyCallback callback,
-                                                void* param,
-                                                HAL_Bool initialNotify);
-void HALSIM_CancelEncoderMaxPeriodCallback(int32_t index, int32_t uid);
-double HALSIM_GetEncoderMaxPeriod(int32_t index);
-void HALSIM_SetEncoderMaxPeriod(int32_t index, double maxPeriod);
-
-int32_t HALSIM_RegisterEncoderDirectionCallback(int32_t index,
-                                                HAL_NotifyCallback callback,
-                                                void* param,
-                                                HAL_Bool initialNotify);
-void HALSIM_CancelEncoderDirectionCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetEncoderDirection(int32_t index);
-void HALSIM_SetEncoderDirection(int32_t index, HAL_Bool direction);
-
-int32_t HALSIM_RegisterEncoderReverseDirectionCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelEncoderReverseDirectionCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetEncoderReverseDirection(int32_t index);
-void HALSIM_SetEncoderReverseDirection(int32_t index,
-                                       HAL_Bool reverseDirection);
-
-int32_t HALSIM_RegisterEncoderSamplesToAverageCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelEncoderSamplesToAverageCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetEncoderSamplesToAverage(int32_t index);
-void HALSIM_SetEncoderSamplesToAverage(int32_t index, int32_t samplesToAverage);
-
-int32_t HALSIM_RegisterEncoderDistancePerPulseCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelEncoderDistancePerPulseCallback(int32_t index, int32_t uid);
-double HALSIM_GetEncoderDistancePerPulse(int32_t index);
-void HALSIM_SetEncoderDistancePerPulse(int32_t index, double distancePerPulse);
-
-void HALSIM_RegisterEncoderAllCallbacks(int32_t index,
-                                        HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/I2CData.h b/hal/src/main/native/include/mockdata/I2CData.h
deleted file mode 100644
index 32ae3a8..0000000
--- a/hal/src/main/native/include/mockdata/I2CData.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetI2CData(int32_t index);
-
-int32_t HALSIM_RegisterI2CInitializedCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelI2CInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetI2CInitialized(int32_t index);
-void HALSIM_SetI2CInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterI2CReadCallback(int32_t index,
-                                       HAL_BufferCallback callback,
-                                       void* param);
-void HALSIM_CancelI2CReadCallback(int32_t index, int32_t uid);
-
-int32_t HALSIM_RegisterI2CWriteCallback(int32_t index,
-                                        HAL_ConstBufferCallback callback,
-                                        void* param);
-void HALSIM_CancelI2CWriteCallback(int32_t index, int32_t uid);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/MockHooks.h b/hal/src/main/native/include/mockdata/MockHooks.h
deleted file mode 100644
index 318ff21..0000000
--- a/hal/src/main/native/include/mockdata/MockHooks.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "hal/Types.h"
-
-extern "C" {
-void HALSIM_WaitForProgramStart(void);
-void HALSIM_SetProgramStarted(void);
-HAL_Bool HALSIM_GetProgramStarted(void);
-void HALSIM_RestartTiming(void);
-void HALSIM_PauseTiming(void);
-void HALSIM_ResumeTiming(void);
-HAL_Bool HALSIM_IsTimingPaused(void);
-void HALSIM_StepTiming(uint64_t delta);
-
-typedef int32_t (*HALSIM_SendErrorHandler)(
-    HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, const char* details,
-    const char* location, const char* callStack, HAL_Bool printMsg);
-void HALSIM_SetSendError(HALSIM_SendErrorHandler handler);
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/include/mockdata/NotifierData.h b/hal/src/main/native/include/mockdata/NotifierData.h
deleted file mode 100644
index b1ed50f..0000000
--- a/hal/src/main/native/include/mockdata/NotifierData.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-struct HALSIM_NotifierInfo {
-  HAL_NotifierHandle handle;
-  char name[64];
-  uint64_t timeout;
-  HAL_Bool running;
-};
-
-uint64_t HALSIM_GetNextNotifierTimeout(void);
-
-int32_t HALSIM_GetNumNotifiers(void);
-
-/**
- * Gets detailed information about each notifier.
- *
- * @param arr array of information to be filled
- * @param size size of arr
- * @return Number of notifiers; note: may be larger than passed-in size
- */
-int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/NotifyListener.h b/hal/src/main/native/include/mockdata/NotifyListener.h
deleted file mode 100644
index a455803..0000000
--- a/hal/src/main/native/include/mockdata/NotifyListener.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "hal/Value.h"
-
-typedef void (*HAL_NotifyCallback)(const char* name, void* param,
-                                   const struct HAL_Value* value);
-
-typedef void (*HAL_BufferCallback)(const char* name, void* param,
-                                   unsigned char* buffer, unsigned int count);
-
-typedef void (*HAL_ConstBufferCallback)(const char* name, void* param,
-                                        const unsigned char* buffer,
-                                        unsigned int count);
-
-#ifdef __cplusplus
-
-namespace hal {
-
-template <typename CallbackFunction>
-struct HalCallbackListener {
-  HalCallbackListener() = default;
-  HalCallbackListener(void* param_, CallbackFunction callback_)
-      : callback(callback_), param(param_) {}
-
-  explicit operator bool() const { return callback != nullptr; }
-
-  CallbackFunction callback;
-  void* param;
-};
-
-}  // namespace hal
-
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/PCMData.h b/hal/src/main/native/include/mockdata/PCMData.h
deleted file mode 100644
index 66b1ec2..0000000
--- a/hal/src/main/native/include/mockdata/PCMData.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetPCMData(int32_t index);
-int32_t HALSIM_RegisterPCMSolenoidInitializedCallback(
-    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelPCMSolenoidInitializedCallback(int32_t index, int32_t channel,
-                                                 int32_t uid);
-HAL_Bool HALSIM_GetPCMSolenoidInitialized(int32_t index, int32_t channel);
-void HALSIM_SetPCMSolenoidInitialized(int32_t index, int32_t channel,
-                                      HAL_Bool solenoidInitialized);
-
-int32_t HALSIM_RegisterPCMSolenoidOutputCallback(int32_t index, int32_t channel,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelPCMSolenoidOutputCallback(int32_t index, int32_t channel,
-                                            int32_t uid);
-HAL_Bool HALSIM_GetPCMSolenoidOutput(int32_t index, int32_t channel);
-void HALSIM_SetPCMSolenoidOutput(int32_t index, int32_t channel,
-                                 HAL_Bool solenoidOutput);
-
-int32_t HALSIM_RegisterPCMCompressorInitializedCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelPCMCompressorInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMCompressorInitialized(int32_t index);
-void HALSIM_SetPCMCompressorInitialized(int32_t index,
-                                        HAL_Bool compressorInitialized);
-
-int32_t HALSIM_RegisterPCMCompressorOnCallback(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-void HALSIM_CancelPCMCompressorOnCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMCompressorOn(int32_t index);
-void HALSIM_SetPCMCompressorOn(int32_t index, HAL_Bool compressorOn);
-
-int32_t HALSIM_RegisterPCMClosedLoopEnabledCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelPCMClosedLoopEnabledCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMClosedLoopEnabled(int32_t index);
-void HALSIM_SetPCMClosedLoopEnabled(int32_t index, HAL_Bool closedLoopEnabled);
-
-int32_t HALSIM_RegisterPCMPressureSwitchCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelPCMPressureSwitchCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMPressureSwitch(int32_t index);
-void HALSIM_SetPCMPressureSwitch(int32_t index, HAL_Bool pressureSwitch);
-
-int32_t HALSIM_RegisterPCMCompressorCurrentCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelPCMCompressorCurrentCallback(int32_t index, int32_t uid);
-double HALSIM_GetPCMCompressorCurrent(int32_t index);
-void HALSIM_SetPCMCompressorCurrent(int32_t index, double compressorCurrent);
-
-void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values);
-void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values);
-
-void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-
-void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/PDPData.h b/hal/src/main/native/include/mockdata/PDPData.h
deleted file mode 100644
index 8315e3c..0000000
--- a/hal/src/main/native/include/mockdata/PDPData.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetPDPData(int32_t index);
-int32_t HALSIM_RegisterPDPInitializedCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelPDPInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPDPInitialized(int32_t index);
-void HALSIM_SetPDPInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterPDPTemperatureCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelPDPTemperatureCallback(int32_t index, int32_t uid);
-double HALSIM_GetPDPTemperature(int32_t index);
-void HALSIM_SetPDPTemperature(int32_t index, double temperature);
-
-int32_t HALSIM_RegisterPDPVoltageCallback(int32_t index,
-                                          HAL_NotifyCallback callback,
-                                          void* param, HAL_Bool initialNotify);
-void HALSIM_CancelPDPVoltageCallback(int32_t index, int32_t uid);
-double HALSIM_GetPDPVoltage(int32_t index);
-void HALSIM_SetPDPVoltage(int32_t index, double voltage);
-
-int32_t HALSIM_RegisterPDPCurrentCallback(int32_t index, int32_t channel,
-                                          HAL_NotifyCallback callback,
-                                          void* param, HAL_Bool initialNotify);
-void HALSIM_CancelPDPCurrentCallback(int32_t index, int32_t channel,
-                                     int32_t uid);
-double HALSIM_GetPDPCurrent(int32_t index, int32_t channel);
-void HALSIM_SetPDPCurrent(int32_t index, int32_t channel, double current);
-
-void HALSIM_GetPDPAllCurrents(int32_t index, double* currents);
-void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents);
-
-void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/PWMData.h b/hal/src/main/native/include/mockdata/PWMData.h
deleted file mode 100644
index 2a8c63d..0000000
--- a/hal/src/main/native/include/mockdata/PWMData.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetPWMData(int32_t index);
-int32_t HALSIM_RegisterPWMInitializedCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelPWMInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPWMInitialized(int32_t index);
-void HALSIM_SetPWMInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterPWMRawValueCallback(int32_t index,
-                                           HAL_NotifyCallback callback,
-                                           void* param, HAL_Bool initialNotify);
-void HALSIM_CancelPWMRawValueCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetPWMRawValue(int32_t index);
-void HALSIM_SetPWMRawValue(int32_t index, int32_t rawValue);
-
-int32_t HALSIM_RegisterPWMSpeedCallback(int32_t index,
-                                        HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-void HALSIM_CancelPWMSpeedCallback(int32_t index, int32_t uid);
-double HALSIM_GetPWMSpeed(int32_t index);
-void HALSIM_SetPWMSpeed(int32_t index, double speed);
-
-int32_t HALSIM_RegisterPWMPositionCallback(int32_t index,
-                                           HAL_NotifyCallback callback,
-                                           void* param, HAL_Bool initialNotify);
-void HALSIM_CancelPWMPositionCallback(int32_t index, int32_t uid);
-double HALSIM_GetPWMPosition(int32_t index);
-void HALSIM_SetPWMPosition(int32_t index, double position);
-
-int32_t HALSIM_RegisterPWMPeriodScaleCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelPWMPeriodScaleCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetPWMPeriodScale(int32_t index);
-void HALSIM_SetPWMPeriodScale(int32_t index, int32_t periodScale);
-
-int32_t HALSIM_RegisterPWMZeroLatchCallback(int32_t index,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify);
-void HALSIM_CancelPWMZeroLatchCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPWMZeroLatch(int32_t index);
-void HALSIM_SetPWMZeroLatch(int32_t index, HAL_Bool zeroLatch);
-
-void HALSIM_RegisterPWMAllCallbacks(int32_t index, HAL_NotifyCallback callback,
-                                    void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/RelayData.h b/hal/src/main/native/include/mockdata/RelayData.h
deleted file mode 100644
index c0b853d..0000000
--- a/hal/src/main/native/include/mockdata/RelayData.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetRelayData(int32_t index);
-int32_t HALSIM_RegisterRelayInitializedForwardCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelRelayInitializedForwardCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRelayInitializedForward(int32_t index);
-void HALSIM_SetRelayInitializedForward(int32_t index,
-                                       HAL_Bool initializedForward);
-
-int32_t HALSIM_RegisterRelayInitializedReverseCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelRelayInitializedReverseCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRelayInitializedReverse(int32_t index);
-void HALSIM_SetRelayInitializedReverse(int32_t index,
-                                       HAL_Bool initializedReverse);
-
-int32_t HALSIM_RegisterRelayForwardCallback(int32_t index,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify);
-void HALSIM_CancelRelayForwardCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRelayForward(int32_t index);
-void HALSIM_SetRelayForward(int32_t index, HAL_Bool forward);
-
-int32_t HALSIM_RegisterRelayReverseCallback(int32_t index,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify);
-void HALSIM_CancelRelayReverseCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRelayReverse(int32_t index);
-void HALSIM_SetRelayReverse(int32_t index, HAL_Bool reverse);
-
-void HALSIM_RegisterRelayAllCallbacks(int32_t index,
-                                      HAL_NotifyCallback callback, void* param,
-                                      HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/RoboRioData.h b/hal/src/main/native/include/mockdata/RoboRioData.h
deleted file mode 100644
index 3acb0ec..0000000
--- a/hal/src/main/native/include/mockdata/RoboRioData.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetRoboRioData(int32_t index);
-int32_t HALSIM_RegisterRoboRioFPGAButtonCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioFPGAButtonCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRoboRioFPGAButton(int32_t index);
-void HALSIM_SetRoboRioFPGAButton(int32_t index, HAL_Bool fPGAButton);
-
-int32_t HALSIM_RegisterRoboRioVInVoltageCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioVInVoltageCallback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioVInVoltage(int32_t index);
-void HALSIM_SetRoboRioVInVoltage(int32_t index, double vInVoltage);
-
-int32_t HALSIM_RegisterRoboRioVInCurrentCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioVInCurrentCallback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioVInCurrent(int32_t index);
-void HALSIM_SetRoboRioVInCurrent(int32_t index, double vInCurrent);
-
-int32_t HALSIM_RegisterRoboRioUserVoltage6VCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserVoltage6VCallback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioUserVoltage6V(int32_t index);
-void HALSIM_SetRoboRioUserVoltage6V(int32_t index, double userVoltage6V);
-
-int32_t HALSIM_RegisterRoboRioUserCurrent6VCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserCurrent6VCallback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioUserCurrent6V(int32_t index);
-void HALSIM_SetRoboRioUserCurrent6V(int32_t index, double userCurrent6V);
-
-int32_t HALSIM_RegisterRoboRioUserActive6VCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserActive6VCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRoboRioUserActive6V(int32_t index);
-void HALSIM_SetRoboRioUserActive6V(int32_t index, HAL_Bool userActive6V);
-
-int32_t HALSIM_RegisterRoboRioUserVoltage5VCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserVoltage5VCallback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioUserVoltage5V(int32_t index);
-void HALSIM_SetRoboRioUserVoltage5V(int32_t index, double userVoltage5V);
-
-int32_t HALSIM_RegisterRoboRioUserCurrent5VCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserCurrent5VCallback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioUserCurrent5V(int32_t index);
-void HALSIM_SetRoboRioUserCurrent5V(int32_t index, double userCurrent5V);
-
-int32_t HALSIM_RegisterRoboRioUserActive5VCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserActive5VCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRoboRioUserActive5V(int32_t index);
-void HALSIM_SetRoboRioUserActive5V(int32_t index, HAL_Bool userActive5V);
-
-int32_t HALSIM_RegisterRoboRioUserVoltage3V3Callback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserVoltage3V3Callback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioUserVoltage3V3(int32_t index);
-void HALSIM_SetRoboRioUserVoltage3V3(int32_t index, double userVoltage3V3);
-
-int32_t HALSIM_RegisterRoboRioUserCurrent3V3Callback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserCurrent3V3Callback(int32_t index, int32_t uid);
-double HALSIM_GetRoboRioUserCurrent3V3(int32_t index);
-void HALSIM_SetRoboRioUserCurrent3V3(int32_t index, double userCurrent3V3);
-
-int32_t HALSIM_RegisterRoboRioUserActive3V3Callback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserActive3V3Callback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetRoboRioUserActive3V3(int32_t index);
-void HALSIM_SetRoboRioUserActive3V3(int32_t index, HAL_Bool userActive3V3);
-
-int32_t HALSIM_RegisterRoboRioUserFaults6VCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserFaults6VCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetRoboRioUserFaults6V(int32_t index);
-void HALSIM_SetRoboRioUserFaults6V(int32_t index, int32_t userFaults6V);
-
-int32_t HALSIM_RegisterRoboRioUserFaults5VCallback(int32_t index,
-                                                   HAL_NotifyCallback callback,
-                                                   void* param,
-                                                   HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserFaults5VCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetRoboRioUserFaults5V(int32_t index);
-void HALSIM_SetRoboRioUserFaults5V(int32_t index, int32_t userFaults5V);
-
-int32_t HALSIM_RegisterRoboRioUserFaults3V3Callback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelRoboRioUserFaults3V3Callback(int32_t index, int32_t uid);
-int32_t HALSIM_GetRoboRioUserFaults3V3(int32_t index);
-void HALSIM_SetRoboRioUserFaults3V3(int32_t index, int32_t userFaults3V3);
-
-void HALSIM_RegisterRoboRioAllCallbacks(int32_t index,
-                                        HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/SPIAccelerometerData.h b/hal/src/main/native/include/mockdata/SPIAccelerometerData.h
deleted file mode 100644
index c68da45..0000000
--- a/hal/src/main/native/include/mockdata/SPIAccelerometerData.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetSPIAccelerometerData(int32_t index);
-int32_t HALSIM_RegisterSPIAccelerometerActiveCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelSPIAccelerometerActiveCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetSPIAccelerometerActive(int32_t index);
-void HALSIM_SetSPIAccelerometerActive(int32_t index, HAL_Bool active);
-
-int32_t HALSIM_RegisterSPIAccelerometerRangeCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelSPIAccelerometerRangeCallback(int32_t index, int32_t uid);
-int32_t HALSIM_GetSPIAccelerometerRange(int32_t index);
-void HALSIM_SetSPIAccelerometerRange(int32_t index, int32_t range);
-
-int32_t HALSIM_RegisterSPIAccelerometerXCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelSPIAccelerometerXCallback(int32_t index, int32_t uid);
-double HALSIM_GetSPIAccelerometerX(int32_t index);
-void HALSIM_SetSPIAccelerometerX(int32_t index, double x);
-
-int32_t HALSIM_RegisterSPIAccelerometerYCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelSPIAccelerometerYCallback(int32_t index, int32_t uid);
-double HALSIM_GetSPIAccelerometerY(int32_t index);
-void HALSIM_SetSPIAccelerometerY(int32_t index, double y);
-
-int32_t HALSIM_RegisterSPIAccelerometerZCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelSPIAccelerometerZCallback(int32_t index, int32_t uid);
-double HALSIM_GetSPIAccelerometerZ(int32_t index);
-void HALSIM_SetSPIAccelerometerZ(int32_t index, double z);
-
-void HALSIM_RegisterSPIAccelerometerAllCallbcaks(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/SPIData.h b/hal/src/main/native/include/mockdata/SPIData.h
deleted file mode 100644
index da21643..0000000
--- a/hal/src/main/native/include/mockdata/SPIData.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-
-typedef void (*HAL_SpiReadAutoReceiveBufferCallback)(const char* name,
-                                                     void* param,
-                                                     uint32_t* buffer,
-                                                     int32_t numToRead,
-                                                     int32_t* outputCount);
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetSPIData(int32_t index);
-
-int32_t HALSIM_RegisterSPIInitializedCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelSPIInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetSPIInitialized(int32_t index);
-void HALSIM_SetSPIInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterSPIReadCallback(int32_t index,
-                                       HAL_BufferCallback callback,
-                                       void* param);
-void HALSIM_CancelSPIReadCallback(int32_t index, int32_t uid);
-
-int32_t HALSIM_RegisterSPIWriteCallback(int32_t index,
-                                        HAL_ConstBufferCallback callback,
-                                        void* param);
-void HALSIM_CancelSPIWriteCallback(int32_t index, int32_t uid);
-
-int32_t HALSIM_RegisterSPIReadAutoReceivedDataCallback(
-    int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
-void HALSIM_CancelSPIReadAutoReceivedDataCallback(int32_t index, int32_t uid);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

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

diff --git a/hal/src/main/native/include/mockdata/SimCallbackRegistry.h b/hal/src/main/native/include/mockdata/SimCallbackRegistry.h
deleted file mode 100644
index 3e1aeb0..0000000
--- a/hal/src/main/native/include/mockdata/SimCallbackRegistry.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include <wpi/Compiler.h>
-#include <wpi/UidVector.h>
-#include <wpi/spinlock.h>
-
-#include "mockdata/NotifyListener.h"
-
-namespace hal {
-
-namespace impl {
-
-class SimCallbackRegistryBase {
- public:
-  using RawFunctor = void (*)();
-
- protected:
-  using CallbackVector = wpi::UidVector<HalCallbackListener<RawFunctor>, 4>;
-
- public:
-  void Cancel(int32_t uid) {
-    std::scoped_lock lock(m_mutex);
-    if (m_callbacks) m_callbacks->erase(uid - 1);
-  }
-
-  void Reset() {
-    std::scoped_lock lock(m_mutex);
-    DoReset();
-  }
-
-  wpi::recursive_spinlock& GetMutex() { return m_mutex; }
-
- protected:
-  int32_t DoRegister(RawFunctor callback, void* param) {
-    // Must return -1 on a null callback for error handling
-    if (callback == nullptr) return -1;
-    if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
-    return m_callbacks->emplace_back(param, callback) + 1;
-  }
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE void DoReset() {
-    if (m_callbacks) m_callbacks->clear();
-  }
-
-  mutable wpi::recursive_spinlock m_mutex;
-  std::unique_ptr<CallbackVector> m_callbacks;
-};
-
-}  // namespace impl
-
-/**
- * Simulation callback registry.  Provides callback functionality.
- *
- * @tparam CallbackFunction callback function type (e.g. HAL_BufferCallback)
- * @tparam GetName function that returns a const char* for the name
- */
-template <typename CallbackFunction, const char* (*GetName)()>
-class SimCallbackRegistry : public impl::SimCallbackRegistryBase {
- public:
-  int32_t Register(CallbackFunction callback, void* param) {
-    std::scoped_lock lock(m_mutex);
-    return DoRegister(reinterpret_cast<RawFunctor>(callback), param);
-  }
-
-  template <typename... U>
-  void Invoke(U&&... u) const {
-#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
-    std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
-#else
-    std::scoped_lock lock(m_mutex);
-#endif
-    if (m_callbacks) {
-      const char* name = GetName();
-      for (auto&& cb : *m_callbacks)
-        reinterpret_cast<CallbackFunction>(cb.callback)(name, cb.param,
-                                                        std::forward<U>(u)...);
-    }
-  }
-
-  template <typename... U>
-  LLVM_ATTRIBUTE_ALWAYS_INLINE void operator()(U&&... u) const {
-    return Invoke(std::forward<U>(u)...);
-  }
-};
-
-/**
- * Define a name functor for use with SimCallbackRegistry.
- * This creates a function named GetNAMEName() that returns "NAME".
- * @param NAME the name to return
- */
-#define HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(NAME)           \
-  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr const char* \
-      Get##NAME##Name() {                                   \
-    return #NAME;                                           \
-  }
-
-/**
- * Define a standard C API for SimCallbackRegistry.
- *
- * Functions defined:
- * - int32 NS_RegisterCAPINAMECallback(
- *      int32_t index, TYPE callback, void* param)
- * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
- *
- * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
- * @param NS the "namespace" (e.g. HALSIM)
- * @param CAPINAME the C API name (usually first letter capitalized)
- * @param DATA the backing data array
- * @param LOWERNAME the lowercase name of the backing data registry
- */
-#define HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, NS, CAPINAME, DATA,     \
-                                            LOWERNAME)                    \
-  int32_t NS##_Register##CAPINAME##Callback(int32_t index, TYPE callback, \
-                                            void* param) {                \
-    return DATA[index].LOWERNAME.Register(callback, param);               \
-  }                                                                       \
-                                                                          \
-  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) {      \
-    DATA[index].LOWERNAME.Cancel(uid);                                    \
-  }
-
-/**
- * Define a standard C API for SimCallbackRegistry (no index variant).
- *
- * Functions defined:
- * - int32 NS_RegisterCAPINAMECallback(TYPE callback, void* param)
- * - void NS_CancelCAPINAMECallback(int32_t uid)
- *
- * @param TYPE the underlying callback type (e.g. HAL_BufferCallback)
- * @param NS the "namespace" (e.g. HALSIM)
- * @param CAPINAME the C API name (usually first letter capitalized)
- * @param DATA the backing data pointer
- * @param LOWERNAME the lowercase name of the backing data registry
- */
-#define HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI_NOINDEX(TYPE, NS, CAPINAME, DATA, \
-                                                    LOWERNAME)                \
-  int32_t NS##_Register##CAPINAME##Callback(TYPE callback, void* param) {     \
-    return DATA->LOWERNAME.Register(callback, param);                         \
-  }                                                                           \
-                                                                              \
-  void NS##_Cancel##CAPINAME##Callback(int32_t uid) {                         \
-    DATA->LOWERNAME.Cancel(uid);                                              \
-  }
-
-}  // namespace hal

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

diff --git a/hal/src/main/native/include/mockdata/SimDataValue.h b/hal/src/main/native/include/mockdata/SimDataValue.h
deleted file mode 100644
index b6723bb..0000000
--- a/hal/src/main/native/include/mockdata/SimDataValue.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <wpi/Compiler.h>
-#include <wpi/UidVector.h>
-#include <wpi/spinlock.h>
-
-#include "mockdata/NotifyListener.h"
-#include "mockdata/SimCallbackRegistry.h"
-
-namespace hal {
-
-namespace impl {
-template <typename T, HAL_Value (*MakeValue)(T)>
-class SimDataValueBase : protected SimCallbackRegistryBase {
- public:
-  explicit SimDataValueBase(T value) : m_value(value) {}
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE void CancelCallback(int32_t uid) { Cancel(uid); }
-
-  T Get() const {
-    std::scoped_lock lock(m_mutex);
-    return m_value;
-  }
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE operator T() const { return Get(); }
-
-  void Reset(T value) {
-    std::scoped_lock lock(m_mutex);
-    DoReset();
-    m_value = value;
-  }
-
-  wpi::recursive_spinlock& GetMutex() { return m_mutex; }
-
- protected:
-  int32_t DoRegisterCallback(HAL_NotifyCallback callback, void* param,
-                             HAL_Bool initialNotify, const char* name) {
-    std::unique_lock lock(m_mutex);
-    int32_t newUid = DoRegister(reinterpret_cast<RawFunctor>(callback), param);
-    if (newUid == -1) return -1;
-    if (initialNotify) {
-      // We know that the callback is not null because of earlier null check
-      HAL_Value value = MakeValue(m_value);
-      lock.unlock();
-      callback(name, param, &value);
-    }
-    return newUid + 1;
-  }
-
-  void DoSet(T value, const char* name) {
-    std::scoped_lock lock(this->m_mutex);
-    if (m_value != value) {
-      m_value = value;
-      if (m_callbacks) {
-        HAL_Value halValue = MakeValue(value);
-        for (auto&& cb : *m_callbacks)
-          reinterpret_cast<HAL_NotifyCallback>(cb.callback)(name, cb.param,
-                                                            &halValue);
-      }
-    }
-  }
-
-  T m_value;
-};
-}  // namespace impl
-
-/**
- * Simulation data value wrapper.  Provides callback functionality when the
- * data value is changed.
- *
- * @tparam T value type (e.g. double)
- * @tparam MakeValue function that takes a T and returns a HAL_Value
- * @tparam GetName function that returns a const char* for the name
- * @tparam GetDefault function that returns the default T (used only for
- *                    default constructor)
- */
-template <typename T, HAL_Value (*MakeValue)(T), const char* (*GetName)(),
-          T (*GetDefault)() = nullptr>
-class SimDataValue final : public impl::SimDataValueBase<T, MakeValue> {
- public:
-  SimDataValue()
-      : impl::SimDataValueBase<T, MakeValue>(
-            GetDefault != nullptr ? GetDefault() : T()) {}
-  explicit SimDataValue(T value)
-      : impl::SimDataValueBase<T, MakeValue>(value) {}
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE int32_t RegisterCallback(
-      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {
-    return this->DoRegisterCallback(callback, param, initialNotify, GetName());
-  }
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE void Set(T value) {
-    this->DoSet(value, GetName());
-  }
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE SimDataValue& operator=(T value) {
-    Set(value);
-    return *this;
-  }
-};
-
-/**
- * Define a name functor for use with SimDataValue.
- * This creates a function named GetNAMEName() that returns "NAME".
- * @param NAME the name to return
- */
-#define HAL_SIMDATAVALUE_DEFINE_NAME(NAME)                  \
-  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr const char* \
-      Get##NAME##Name() {                                   \
-    return #NAME;                                           \
-  }
-
-/**
- * Define a standard C API for simulation data.
- *
- * Functions defined:
- * - int32 NS_RegisterCAPINAMECallback(
- *      int32_t index, HAL_NotifyCallback callback, void* param,
- *      HAL_Bool initialNotify)
- * - void NS_CancelCAPINAMECallback(int32_t index, int32_t uid)
- * - TYPE NS_GetCAPINAME(int32_t index)
- * - void NS_SetCAPINAME(int32_t index, TYPE value)
- *
- * @param TYPE the underlying value type (e.g. double)
- * @param NS the "namespace" (e.g. HALSIM)
- * @param CAPINAME the C API name (usually first letter capitalized)
- * @param DATA the backing data array
- * @param LOWERNAME the lowercase name of the backing data variable
- */
-#define HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, NS, CAPINAME, DATA, LOWERNAME)  \
-  int32_t NS##_Register##CAPINAME##Callback(                               \
-      int32_t index, HAL_NotifyCallback callback, void* param,             \
-      HAL_Bool initialNotify) {                                            \
-    return DATA[index].LOWERNAME.RegisterCallback(callback, param,         \
-                                                  initialNotify);          \
-  }                                                                        \
-                                                                           \
-  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t uid) {       \
-    DATA[index].LOWERNAME.CancelCallback(uid);                             \
-  }                                                                        \
-                                                                           \
-  TYPE NS##_Get##CAPINAME(int32_t index) { return DATA[index].LOWERNAME; } \
-                                                                           \
-  void NS##_Set##CAPINAME(int32_t index, TYPE LOWERNAME) {                 \
-    DATA[index].LOWERNAME = LOWERNAME;                                     \
-  }
-
-/**
- * Define a standard C API for simulation data (channel variant).
- *
- * Functions defined:
- * - int32 NS_RegisterCAPINAMECallback(
- *      int32_t index, int32_t channel, HAL_NotifyCallback callback,
- *      void* param, HAL_Bool initialNotify)
- * - void NS_CancelCAPINAMECallback(int32_t index, int32_t channel, int32_t uid)
- * - TYPE NS_GetCAPINAME(int32_t index, int32_t channel)
- * - void NS_SetCAPINAME(int32_t index, int32_t channel, TYPE value)
- *
- * @param TYPE the underlying value type (e.g. double)
- * @param NS the "namespace" (e.g. HALSIM)
- * @param CAPINAME the C API name (usually first letter capitalized)
- * @param DATA the backing data array
- * @param LOWERNAME the lowercase name of the backing data variable array
- */
-#define HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(TYPE, NS, CAPINAME, DATA,      \
-                                             LOWERNAME)                     \
-  int32_t NS##_Register##CAPINAME##Callback(                                \
-      int32_t index, int32_t channel, HAL_NotifyCallback callback,          \
-      void* param, HAL_Bool initialNotify) {                                \
-    return DATA[index].LOWERNAME[channel].RegisterCallback(callback, param, \
-                                                           initialNotify);  \
-  }                                                                         \
-                                                                            \
-  void NS##_Cancel##CAPINAME##Callback(int32_t index, int32_t channel,      \
-                                       int32_t uid) {                       \
-    DATA[index].LOWERNAME[channel].CancelCallback(uid);                     \
-  }                                                                         \
-                                                                            \
-  TYPE NS##_Get##CAPINAME(int32_t index, int32_t channel) {                 \
-    return DATA[index].LOWERNAME[channel];                                  \
-  }                                                                         \
-                                                                            \
-  void NS##_Set##CAPINAME(int32_t index, int32_t channel, TYPE LOWERNAME) { \
-    DATA[index].LOWERNAME[channel] = LOWERNAME;                             \
-  }
-
-/**
- * Define a standard C API for simulation data (no index variant).
- *
- * Functions defined:
- * - int32 NS_RegisterCAPINAMECallback(
- *      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify)
- * - void NS_CancelCAPINAMECallback(int32_t uid)
- * - TYPE NS_GetCAPINAME(void)
- * - void NS_SetCAPINAME(TYPE value)
- *
- * @param TYPE the underlying value type (e.g. double)
- * @param NS the "namespace" (e.g. HALSIM)
- * @param CAPINAME the C API name (usually first letter capitalized)
- * @param DATA the backing data pointer
- * @param LOWERNAME the lowercase name of the backing data variable
- */
-#define HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, NS, CAPINAME, DATA,       \
-                                             LOWERNAME)                      \
-  int32_t NS##_Register##CAPINAME##Callback(                                 \
-      HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {    \
-    return DATA->LOWERNAME.RegisterCallback(callback, param, initialNotify); \
-  }                                                                          \
-                                                                             \
-  void NS##_Cancel##CAPINAME##Callback(int32_t uid) {                        \
-    DATA->LOWERNAME.CancelCallback(uid);                                     \
-  }                                                                          \
-                                                                             \
-  TYPE NS##_Get##CAPINAME(void) { return DATA->LOWERNAME; }                  \
-                                                                             \
-  void NS##_Set##CAPINAME(TYPE LOWERNAME) { DATA->LOWERNAME = LOWERNAME; }
-
-}  // namespace hal

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

diff --git a/hal/src/main/native/include/mockdata/SimDeviceData.h b/hal/src/main/native/include/mockdata/SimDeviceData.h
deleted file mode 100644
index 44398d7..0000000
--- a/hal/src/main/native/include/mockdata/SimDeviceData.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "NotifyListener.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-
-typedef void (*HALSIM_SimDeviceCallback)(const char* name, void* param,
-                                         HAL_SimDeviceHandle handle);
-
-typedef void (*HALSIM_SimValueCallback)(const char* name, void* param,
-                                        HAL_SimValueHandle handle,
-                                        HAL_Bool readonly,
-                                        const struct HAL_Value* value);
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-int32_t HALSIM_RegisterSimDeviceCreatedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
-    HAL_Bool initialNotify);
-
-void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid);
-
-int32_t HALSIM_RegisterSimDeviceFreedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback);
-
-void HALSIM_CancelSimDeviceFreedCallback(int32_t uid);
-
-HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name);
-
-const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle);
-
-HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle);
-
-void HALSIM_EnumerateSimDevices(const char* prefix, void* param,
-                                HALSIM_SimDeviceCallback callback);
-
-int32_t HALSIM_RegisterSimValueCreatedCallback(HAL_SimDeviceHandle device,
-                                               void* param,
-                                               HALSIM_SimValueCallback callback,
-                                               HAL_Bool initialNotify);
-
-void HALSIM_CancelSimValueCreatedCallback(int32_t uid);
-
-int32_t HALSIM_RegisterSimValueChangedCallback(HAL_SimValueHandle handle,
-                                               void* param,
-                                               HALSIM_SimValueCallback callback,
-                                               HAL_Bool initialNotify);
-
-void HALSIM_CancelSimValueChangedCallback(int32_t uid);
-
-HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
-                                            const char* name);
-
-void HALSIM_EnumerateSimValues(HAL_SimDeviceHandle device, void* param,
-                               HALSIM_SimValueCallback callback);
-
-const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
-                                           int32_t* numOptions);
-
-void HALSIM_ResetSimDeviceData(void);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif

hal/src/main/native/include/simulation/AccelerometerSim.h

diff --git a/hal/src/main/native/include/simulation/AccelerometerSim.h b/hal/src/main/native/include/simulation/AccelerometerSim.h
deleted file mode 100644
index 07860f0..0000000
--- a/hal/src/main/native/include/simulation/AccelerometerSim.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/AccelerometerData.h"
-
-namespace frc {
-namespace sim {
-class AccelerometerSim {
- public:
-  explicit AccelerometerSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
-                                                        bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAccelerometerActiveCallback);
-    store->SetUid(HALSIM_RegisterAccelerometerActiveCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetActive() const { return HALSIM_GetAccelerometerActive(m_index); }
-
-  void SetActive(bool active) {
-    HALSIM_SetAccelerometerActive(m_index, active);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAccelerometerRangeCallback);
-    store->SetUid(HALSIM_RegisterAccelerometerRangeCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  HAL_AccelerometerRange GetRange() const {
-    return HALSIM_GetAccelerometerRange(m_index);
-  }
-
-  void SetRange(HAL_AccelerometerRange range) {
-    HALSIM_SetAccelerometerRange(m_index, range);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
-                                                   bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAccelerometerXCallback);
-    store->SetUid(HALSIM_RegisterAccelerometerXCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetX() const { return HALSIM_GetAccelerometerX(m_index); }
-
-  void SetX(double x) { HALSIM_SetAccelerometerX(m_index, x); }
-
-  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
-                                                   bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAccelerometerYCallback);
-    store->SetUid(HALSIM_RegisterAccelerometerYCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetY() const { return HALSIM_GetAccelerometerY(m_index); }
-
-  void SetY(double y) { HALSIM_SetAccelerometerY(m_index, y); }
-
-  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
-                                                   bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAccelerometerZCallback);
-    store->SetUid(HALSIM_RegisterAccelerometerZCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetZ() const { return HALSIM_GetAccelerometerZ(m_index); }
-
-  void SetZ(double z) { HALSIM_SetAccelerometerZ(m_index, z); }
-
-  void ResetData() { HALSIM_ResetAccelerometerData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/AnalogGyroSim.h

diff --git a/hal/src/main/native/include/simulation/AnalogGyroSim.h b/hal/src/main/native/include/simulation/AnalogGyroSim.h
deleted file mode 100644
index b2cdb85..0000000
--- a/hal/src/main/native/include/simulation/AnalogGyroSim.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/AnalogGyroData.h"
-
-namespace frc {
-namespace sim {
-class AnalogGyroSim {
- public:
-  explicit AnalogGyroSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterAngleCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogGyroAngleCallback);
-    store->SetUid(HALSIM_RegisterAnalogGyroAngleCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetAngle() const { return HALSIM_GetAnalogGyroAngle(m_index); }
-
-  void SetAngle(double angle) { HALSIM_SetAnalogGyroAngle(m_index, angle); }
-
-  std::unique_ptr<CallbackStore> RegisterRateCallback(NotifyCallback callback,
-                                                      bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogGyroRateCallback);
-    store->SetUid(HALSIM_RegisterAnalogGyroRateCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetRate() const { return HALSIM_GetAnalogGyroRate(m_index); }
-
-  void SetRate(double rate) { HALSIM_SetAnalogGyroRate(m_index, rate); }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogGyroInitializedCallback);
-    store->SetUid(HALSIM_RegisterAnalogGyroInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const {
-    return HALSIM_GetAnalogGyroInitialized(m_index);
-  }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetAnalogGyroInitialized(m_index, initialized);
-  }
-
-  void ResetData() { HALSIM_ResetAnalogGyroData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/AnalogInSim.h

diff --git a/hal/src/main/native/include/simulation/AnalogInSim.h b/hal/src/main/native/include/simulation/AnalogInSim.h
deleted file mode 100644
index 2a1bbea..0000000
--- a/hal/src/main/native/include/simulation/AnalogInSim.h
+++ /dev/null
@@ -1,177 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/AnalogInData.h"
-
-namespace frc {
-namespace sim {
-class AnalogInSim {
- public:
-  explicit AnalogInSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogInInitializedCallback);
-    store->SetUid(HALSIM_RegisterAnalogInInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const { return HALSIM_GetAnalogInInitialized(m_index); }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetAnalogInInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterAverageBitsCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogInAverageBitsCallback);
-    store->SetUid(HALSIM_RegisterAnalogInAverageBitsCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetAverageBits() const { return HALSIM_GetAnalogInAverageBits(m_index); }
-
-  void SetAverageBits(int averageBits) {
-    HALSIM_SetAnalogInAverageBits(m_index, averageBits);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterOversampleBitsCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogInOversampleBitsCallback);
-    store->SetUid(HALSIM_RegisterAnalogInOversampleBitsCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetOversampleBits() const {
-    return HALSIM_GetAnalogInOversampleBits(m_index);
-  }
-
-  void SetOversampleBits(int oversampleBits) {
-    HALSIM_SetAnalogInOversampleBits(m_index, oversampleBits);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogInVoltageCallback);
-    store->SetUid(HALSIM_RegisterAnalogInVoltageCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetVoltage() const { return HALSIM_GetAnalogInVoltage(m_index); }
-
-  void SetVoltage(double voltage) {
-    HALSIM_SetAnalogInVoltage(m_index, voltage);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterAccumulatorInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback,
-        &HALSIM_CancelAnalogInAccumulatorInitializedCallback);
-    store->SetUid(HALSIM_RegisterAnalogInAccumulatorInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetAccumulatorInitialized() const {
-    return HALSIM_GetAnalogInAccumulatorInitialized(m_index);
-  }
-
-  void SetAccumulatorInitialized(bool accumulatorInitialized) {
-    HALSIM_SetAnalogInAccumulatorInitialized(m_index, accumulatorInitialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterAccumulatorValueCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorValueCallback);
-    store->SetUid(HALSIM_RegisterAnalogInAccumulatorValueCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int64_t GetAccumulatorValue() const {
-    return HALSIM_GetAnalogInAccumulatorValue(m_index);
-  }
-
-  void SetAccumulatorValue(int64_t accumulatorValue) {
-    HALSIM_SetAnalogInAccumulatorValue(m_index, accumulatorValue);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterAccumulatorCountCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCountCallback);
-    store->SetUid(HALSIM_RegisterAnalogInAccumulatorCountCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int64_t GetAccumulatorCount() const {
-    return HALSIM_GetAnalogInAccumulatorCount(m_index);
-  }
-
-  void SetAccumulatorCount(int64_t accumulatorCount) {
-    HALSIM_SetAnalogInAccumulatorCount(m_index, accumulatorCount);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterAccumulatorCenterCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCenterCallback);
-    store->SetUid(HALSIM_RegisterAnalogInAccumulatorCenterCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetAccumulatorCenter() const {
-    return HALSIM_GetAnalogInAccumulatorCenter(m_index);
-  }
-
-  void SetAccumulatorCenter(int accumulatorCenter) {
-    HALSIM_SetAnalogInAccumulatorCenter(m_index, accumulatorCenter);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterAccumulatorDeadbandCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback,
-        &HALSIM_CancelAnalogInAccumulatorDeadbandCallback);
-    store->SetUid(HALSIM_RegisterAnalogInAccumulatorDeadbandCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetAccumulatorDeadband() const {
-    return HALSIM_GetAnalogInAccumulatorDeadband(m_index);
-  }
-
-  void SetAccumulatorDeadband(int accumulatorDeadband) {
-    HALSIM_SetAnalogInAccumulatorDeadband(m_index, accumulatorDeadband);
-  }
-
-  void ResetData() { HALSIM_ResetAnalogInData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/AnalogOutSim.h

diff --git a/hal/src/main/native/include/simulation/AnalogOutSim.h b/hal/src/main/native/include/simulation/AnalogOutSim.h
deleted file mode 100644
index 6bb6e5a..0000000
--- a/hal/src/main/native/include/simulation/AnalogOutSim.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/AnalogOutData.h"
-
-namespace frc {
-namespace sim {
-class AnalogOutSim {
- public:
-  explicit AnalogOutSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogOutVoltageCallback);
-    store->SetUid(HALSIM_RegisterAnalogOutVoltageCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetVoltage() const { return HALSIM_GetAnalogOutVoltage(m_index); }
-
-  void SetVoltage(double voltage) {
-    HALSIM_SetAnalogOutVoltage(m_index, voltage);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogOutInitializedCallback);
-    store->SetUid(HALSIM_RegisterAnalogOutInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const {
-    return HALSIM_GetAnalogOutInitialized(m_index);
-  }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetAnalogOutInitialized(m_index, initialized);
-  }
-
-  void ResetData() { HALSIM_ResetAnalogOutData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/AnalogTriggerSim.h

diff --git a/hal/src/main/native/include/simulation/AnalogTriggerSim.h b/hal/src/main/native/include/simulation/AnalogTriggerSim.h
deleted file mode 100644
index 2c41be1..0000000
--- a/hal/src/main/native/include/simulation/AnalogTriggerSim.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/AnalogTriggerData.h"
-
-namespace frc {
-namespace sim {
-class AnalogTriggerSim {
- public:
-  explicit AnalogTriggerSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelAnalogTriggerInitializedCallback);
-    store->SetUid(HALSIM_RegisterAnalogTriggerInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const {
-    return HALSIM_GetAnalogTriggerInitialized(m_index);
-  }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetAnalogTriggerInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterTriggerLowerBoundCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback,
-        &HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback);
-    store->SetUid(HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetTriggerLowerBound() const {
-    return HALSIM_GetAnalogTriggerTriggerLowerBound(m_index);
-  }
-
-  void SetTriggerLowerBound(double triggerLowerBound) {
-    HALSIM_SetAnalogTriggerTriggerLowerBound(m_index, triggerLowerBound);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterTriggerUpperBoundCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback,
-        &HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback);
-    store->SetUid(HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetTriggerUpperBound() const {
-    return HALSIM_GetAnalogTriggerTriggerUpperBound(m_index);
-  }
-
-  void SetTriggerUpperBound(double triggerUpperBound) {
-    HALSIM_SetAnalogTriggerTriggerUpperBound(m_index, triggerUpperBound);
-  }
-
-  void ResetData() { HALSIM_ResetAnalogTriggerData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/CallbackStore.h

diff --git a/hal/src/main/native/include/simulation/CallbackStore.h b/hal/src/main/native/include/simulation/CallbackStore.h
deleted file mode 100644
index b2d4bdf..0000000
--- a/hal/src/main/native/include/simulation/CallbackStore.h
+++ /dev/null
@@ -1,89 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-
-#include <wpi/StringRef.h>
-
-#include "hal/Value.h"
-
-namespace frc {
-namespace sim {
-
-using NotifyCallback = std::function<void(wpi::StringRef, const HAL_Value*)>;
-typedef void (*CancelCallbackFunc)(int32_t index, int32_t uid);
-typedef void (*CancelCallbackNoIndexFunc)(int32_t uid);
-typedef void (*CancelCallbackChannelFunc)(int32_t index, int32_t channel,
-                                          int32_t uid);
-
-void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
-
-class CallbackStore {
- public:
-  CallbackStore(int32_t i, NotifyCallback cb, CancelCallbackNoIndexFunc ccf) {
-    index = i;
-    callback = cb;
-    this->ccnif = ccf;
-    cancelType = NoIndex;
-  }
-
-  CallbackStore(int32_t i, int32_t u, NotifyCallback cb,
-                CancelCallbackFunc ccf) {
-    index = i;
-    uid = u;
-    callback = cb;
-    this->ccf = ccf;
-    cancelType = Normal;
-  }
-
-  CallbackStore(int32_t i, int32_t c, int32_t u, NotifyCallback cb,
-                CancelCallbackChannelFunc ccf) {
-    index = i;
-    channel = c;
-    uid = u;
-    callback = cb;
-    this->cccf = ccf;
-    cancelType = Channel;
-  }
-
-  ~CallbackStore() {
-    switch (cancelType) {
-      case Normal:
-        ccf(index, uid);
-        break;
-      case Channel:
-        cccf(index, channel, uid);
-        break;
-      case NoIndex:
-        ccnif(uid);
-        break;
-    }
-  }
-
-  void SetUid(int32_t uid) { this->uid = uid; }
-
-  friend void CallbackStoreThunk(const char* name, void* param,
-                                 const HAL_Value* value);
-
- private:
-  int32_t index;
-  int32_t channel;
-  int32_t uid;
-
-  NotifyCallback callback;
-  union {
-    CancelCallbackFunc ccf;
-    CancelCallbackChannelFunc cccf;
-    CancelCallbackNoIndexFunc ccnif;
-  };
-  enum CancelType { Normal, Channel, NoIndex };
-  CancelType cancelType;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/DIOSim.h

diff --git a/hal/src/main/native/include/simulation/DIOSim.h b/hal/src/main/native/include/simulation/DIOSim.h
deleted file mode 100644
index 57079b9..0000000
--- a/hal/src/main/native/include/simulation/DIOSim.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/DIOData.h"
-
-namespace frc {
-namespace sim {
-class DIOSim {
- public:
-  explicit DIOSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDIOInitializedCallback);
-    store->SetUid(HALSIM_RegisterDIOInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const { return HALSIM_GetDIOInitialized(m_index); }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetDIOInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterValueCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDIOValueCallback);
-    store->SetUid(HALSIM_RegisterDIOValueCallback(m_index, &CallbackStoreThunk,
-                                                  store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetValue() const { return HALSIM_GetDIOValue(m_index); }
-
-  void SetValue(bool value) { HALSIM_SetDIOValue(m_index, value); }
-
-  std::unique_ptr<CallbackStore> RegisterPulseLengthCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDIOPulseLengthCallback);
-    store->SetUid(HALSIM_RegisterDIOPulseLengthCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetPulseLength() const { return HALSIM_GetDIOPulseLength(m_index); }
-
-  void SetPulseLength(double pulseLength) {
-    HALSIM_SetDIOPulseLength(m_index, pulseLength);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterIsInputCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDIOIsInputCallback);
-    store->SetUid(HALSIM_RegisterDIOIsInputCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetIsInput() const { return HALSIM_GetDIOIsInput(m_index); }
-
-  void SetIsInput(bool isInput) { HALSIM_SetDIOIsInput(m_index, isInput); }
-
-  std::unique_ptr<CallbackStore> RegisterFilterIndexCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDIOFilterIndexCallback);
-    store->SetUid(HALSIM_RegisterDIOFilterIndexCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetFilterIndex() const { return HALSIM_GetDIOFilterIndex(m_index); }
-
-  void SetFilterIndex(int filterIndex) {
-    HALSIM_SetDIOFilterIndex(m_index, filterIndex);
-  }
-
-  void ResetData() { HALSIM_ResetDIOData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/DigitalPWMSim.h

diff --git a/hal/src/main/native/include/simulation/DigitalPWMSim.h b/hal/src/main/native/include/simulation/DigitalPWMSim.h
deleted file mode 100644
index 5f5769d..0000000
--- a/hal/src/main/native/include/simulation/DigitalPWMSim.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/DigitalPWMData.h"
-
-namespace frc {
-namespace sim {
-class DigitalPWMSim {
- public:
-  explicit DigitalPWMSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDigitalPWMInitializedCallback);
-    store->SetUid(HALSIM_RegisterDigitalPWMInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const {
-    return HALSIM_GetDigitalPWMInitialized(m_index);
-  }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetDigitalPWMInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterDutyCycleCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDigitalPWMDutyCycleCallback);
-    store->SetUid(HALSIM_RegisterDigitalPWMDutyCycleCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetDutyCycle() const { return HALSIM_GetDigitalPWMDutyCycle(m_index); }
-
-  void SetDutyCycle(double dutyCycle) {
-    HALSIM_SetDigitalPWMDutyCycle(m_index, dutyCycle);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterPinCallback(NotifyCallback callback,
-                                                     bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDigitalPWMPinCallback);
-    store->SetUid(HALSIM_RegisterDigitalPWMPinCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetPin() const { return HALSIM_GetDigitalPWMPin(m_index); }
-
-  void SetPin(int pin) { HALSIM_SetDigitalPWMPin(m_index, pin); }
-
-  void ResetData() { HALSIM_ResetDigitalPWMData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/DriverStationSim.h

diff --git a/hal/src/main/native/include/simulation/DriverStationSim.h b/hal/src/main/native/include/simulation/DriverStationSim.h
deleted file mode 100644
index e5071cc..0000000
--- a/hal/src/main/native/include/simulation/DriverStationSim.h
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/DriverStationData.h"
-
-namespace frc {
-namespace sim {
-class DriverStationSim {
- public:
-  std::unique_ptr<CallbackStore> RegisterEnabledCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        -1, callback, &HALSIM_CancelDriverStationEnabledCallback);
-    store->SetUid(HALSIM_RegisterDriverStationEnabledCallback(
-        &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetEnabled() const { return HALSIM_GetDriverStationEnabled(); }
-
-  void SetEnabled(bool enabled) { HALSIM_SetDriverStationEnabled(enabled); }
-
-  std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        -1, callback, &HALSIM_CancelDriverStationAutonomousCallback);
-    store->SetUid(HALSIM_RegisterDriverStationAutonomousCallback(
-        &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetAutonomous() const { return HALSIM_GetDriverStationAutonomous(); }
-
-  void SetAutonomous(bool autonomous) {
-    HALSIM_SetDriverStationAutonomous(autonomous);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterTestCallback(NotifyCallback callback,
-                                                      bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        -1, callback, &HALSIM_CancelDriverStationTestCallback);
-    store->SetUid(HALSIM_RegisterDriverStationTestCallback(
-        &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetTest() const { return HALSIM_GetDriverStationTest(); }
-
-  void SetTest(bool test) { HALSIM_SetDriverStationTest(test); }
-
-  std::unique_ptr<CallbackStore> RegisterEStopCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        -1, callback, &HALSIM_CancelDriverStationEStopCallback);
-    store->SetUid(HALSIM_RegisterDriverStationEStopCallback(
-        &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetEStop() const { return HALSIM_GetDriverStationEStop(); }
-
-  void SetEStop(bool eStop) { HALSIM_SetDriverStationEStop(eStop); }
-
-  std::unique_ptr<CallbackStore> RegisterFmsAttachedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        -1, callback, &HALSIM_CancelDriverStationFmsAttachedCallback);
-    store->SetUid(HALSIM_RegisterDriverStationFmsAttachedCallback(
-        &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetFmsAttached() const { return HALSIM_GetDriverStationFmsAttached(); }
-
-  void SetFmsAttached(bool fmsAttached) {
-    HALSIM_SetDriverStationFmsAttached(fmsAttached);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterDsAttachedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        -1, callback, &HALSIM_CancelDriverStationDsAttachedCallback);
-    store->SetUid(HALSIM_RegisterDriverStationDsAttachedCallback(
-        &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetDsAttached() const { return HALSIM_GetDriverStationDsAttached(); }
-
-  void SetDsAttached(bool dsAttached) {
-    HALSIM_SetDriverStationDsAttached(dsAttached);
-  }
-
-  void NotifyNewData() { HALSIM_NotifyDriverStationNewData(); }
-
-  void ResetData() { HALSIM_ResetDriverStationData(); }
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/DutyCycleSim.h

diff --git a/hal/src/main/native/include/simulation/DutyCycleSim.h b/hal/src/main/native/include/simulation/DutyCycleSim.h
deleted file mode 100644
index f55dfc9..0000000
--- a/hal/src/main/native/include/simulation/DutyCycleSim.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/DutyCycleData.h"
-
-namespace frc {
-namespace sim {
-class DutyCycleSim {
- public:
-  explicit DutyCycleSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDutyCycleInitializedCallback);
-    store->SetUid(HALSIM_RegisterDutyCycleInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const {
-    return HALSIM_GetDutyCycleInitialized(m_index);
-  }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetDutyCycleInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDutyCycleFrequencyCallback);
-    store->SetUid(HALSIM_RegisterDutyCycleFrequencyCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetFrequency() const { return HALSIM_GetDutyCycleFrequency(m_index); }
-
-  void SetFrequency(int count) { HALSIM_SetDutyCycleFrequency(m_index, count); }
-
-  std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
-                                                        bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelDutyCycleOutputCallback);
-    store->SetUid(HALSIM_RegisterDutyCycleOutputCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetOutput() const { return HALSIM_GetDutyCycleOutput(m_index); }
-
-  void SetOutput(double period) { HALSIM_SetDutyCycleOutput(m_index, period); }
-
-  void ResetData() { HALSIM_ResetDutyCycleData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/EncoderSim.h

diff --git a/hal/src/main/native/include/simulation/EncoderSim.h b/hal/src/main/native/include/simulation/EncoderSim.h
deleted file mode 100644
index f80e8a8..0000000
--- a/hal/src/main/native/include/simulation/EncoderSim.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/EncoderData.h"
-
-namespace frc {
-namespace sim {
-class EncoderSim {
- public:
-  explicit EncoderSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderInitializedCallback);
-    store->SetUid(HALSIM_RegisterEncoderInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const { return HALSIM_GetEncoderInitialized(m_index); }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetEncoderInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterCountCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderCountCallback);
-    store->SetUid(HALSIM_RegisterEncoderCountCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetCount() const { return HALSIM_GetEncoderCount(m_index); }
-
-  void SetCount(int count) { HALSIM_SetEncoderCount(m_index, count); }
-
-  std::unique_ptr<CallbackStore> RegisterPeriodCallback(NotifyCallback callback,
-                                                        bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderPeriodCallback);
-    store->SetUid(HALSIM_RegisterEncoderPeriodCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetPeriod() const { return HALSIM_GetEncoderPeriod(m_index); }
-
-  void SetPeriod(double period) { HALSIM_SetEncoderPeriod(m_index, period); }
-
-  std::unique_ptr<CallbackStore> RegisterResetCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderResetCallback);
-    store->SetUid(HALSIM_RegisterEncoderResetCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetReset() const { return HALSIM_GetEncoderReset(m_index); }
-
-  void SetReset(bool reset) { HALSIM_SetEncoderReset(m_index, reset); }
-
-  std::unique_ptr<CallbackStore> RegisterMaxPeriodCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderMaxPeriodCallback);
-    store->SetUid(HALSIM_RegisterEncoderMaxPeriodCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetMaxPeriod() const { return HALSIM_GetEncoderMaxPeriod(m_index); }
-
-  void SetMaxPeriod(double maxPeriod) {
-    HALSIM_SetEncoderMaxPeriod(m_index, maxPeriod);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterDirectionCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderDirectionCallback);
-    store->SetUid(HALSIM_RegisterEncoderDirectionCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetDirection() const { return HALSIM_GetEncoderDirection(m_index); }
-
-  void SetDirection(bool direction) {
-    HALSIM_SetEncoderDirection(m_index, direction);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterReverseDirectionCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderReverseDirectionCallback);
-    store->SetUid(HALSIM_RegisterEncoderReverseDirectionCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetReverseDirection() const {
-    return HALSIM_GetEncoderReverseDirection(m_index);
-  }
-
-  void SetReverseDirection(bool reverseDirection) {
-    HALSIM_SetEncoderReverseDirection(m_index, reverseDirection);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterSamplesToAverageCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderSamplesToAverageCallback);
-    store->SetUid(HALSIM_RegisterEncoderSamplesToAverageCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetSamplesToAverage() const {
-    return HALSIM_GetEncoderSamplesToAverage(m_index);
-  }
-
-  void SetSamplesToAverage(int samplesToAverage) {
-    HALSIM_SetEncoderSamplesToAverage(m_index, samplesToAverage);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterDistancePerPulseCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelEncoderDistancePerPulseCallback);
-    store->SetUid(HALSIM_RegisterEncoderDistancePerPulseCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetDistancePerPulse() const {
-    return HALSIM_GetEncoderDistancePerPulse(m_index);
-  }
-
-  void SetDistancePerPulse(double distancePerPulse) {
-    HALSIM_SetEncoderDistancePerPulse(m_index, distancePerPulse);
-  }
-
-  void ResetData() { HALSIM_ResetEncoderData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/PCMSim.h

diff --git a/hal/src/main/native/include/simulation/PCMSim.h b/hal/src/main/native/include/simulation/PCMSim.h
deleted file mode 100644
index b6fcd5a..0000000
--- a/hal/src/main/native/include/simulation/PCMSim.h
+++ /dev/null
@@ -1,157 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/PCMData.h"
-
-namespace frc {
-namespace sim {
-class PCMSim {
- public:
-  explicit PCMSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterSolenoidInitializedCallback(
-      int channel, NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, channel, -1, callback,
-        &HALSIM_CancelPCMSolenoidInitializedCallback);
-    store->SetUid(HALSIM_RegisterPCMSolenoidInitializedCallback(
-        m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetSolenoidInitialized(int channel) const {
-    return HALSIM_GetPCMSolenoidInitialized(m_index, channel);
-  }
-
-  void SetSolenoidInitialized(int channel, bool solenoidInitialized) {
-    HALSIM_SetPCMSolenoidInitialized(m_index, channel, solenoidInitialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
-      int channel, NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, channel, -1, callback,
-        &HALSIM_CancelPCMSolenoidOutputCallback);
-    store->SetUid(HALSIM_RegisterPCMSolenoidOutputCallback(
-        m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetSolenoidOutput(int channel) const {
-    return HALSIM_GetPCMSolenoidOutput(m_index, channel);
-  }
-
-  void SetSolenoidOutput(int channel, bool solenoidOutput) {
-    HALSIM_SetPCMSolenoidOutput(m_index, channel, solenoidOutput);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterCompressorInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPCMCompressorInitializedCallback);
-    store->SetUid(HALSIM_RegisterPCMCompressorInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetCompressorInitialized() const {
-    return HALSIM_GetPCMCompressorInitialized(m_index);
-  }
-
-  void SetCompressorInitialized(bool compressorInitialized) {
-    HALSIM_SetPCMCompressorInitialized(m_index, compressorInitialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPCMCompressorOnCallback);
-    store->SetUid(HALSIM_RegisterPCMCompressorOnCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetCompressorOn() const { return HALSIM_GetPCMCompressorOn(m_index); }
-
-  void SetCompressorOn(bool compressorOn) {
-    HALSIM_SetPCMCompressorOn(m_index, compressorOn);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterClosedLoopEnabledCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPCMClosedLoopEnabledCallback);
-    store->SetUid(HALSIM_RegisterPCMClosedLoopEnabledCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetClosedLoopEnabled() const {
-    return HALSIM_GetPCMClosedLoopEnabled(m_index);
-  }
-
-  void SetClosedLoopEnabled(bool closedLoopEnabled) {
-    HALSIM_SetPCMClosedLoopEnabled(m_index, closedLoopEnabled);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPCMPressureSwitchCallback);
-    store->SetUid(HALSIM_RegisterPCMPressureSwitchCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetPressureSwitch() const {
-    return HALSIM_GetPCMPressureSwitch(m_index);
-  }
-
-  void SetPressureSwitch(bool pressureSwitch) {
-    HALSIM_SetPCMPressureSwitch(m_index, pressureSwitch);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterCompressorCurrentCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPCMCompressorCurrentCallback);
-    store->SetUid(HALSIM_RegisterPCMCompressorCurrentCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetCompressorCurrent() const {
-    return HALSIM_GetPCMCompressorCurrent(m_index);
-  }
-
-  void SetCompressorCurrent(double compressorCurrent) {
-    HALSIM_SetPCMCompressorCurrent(m_index, compressorCurrent);
-  }
-
-  uint8_t GetAllSolenoidOutputs() {
-    uint8_t ret = 0;
-    HALSIM_GetPCMAllSolenoids(m_index, &ret);
-    return ret;
-  }
-
-  void SetAllSolenoidOutputs(uint8_t outputs) {
-    HALSIM_SetPCMAllSolenoids(m_index, outputs);
-  }
-
-  void ResetData() { HALSIM_ResetPCMData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/PDPSim.h

diff --git a/hal/src/main/native/include/simulation/PDPSim.h b/hal/src/main/native/include/simulation/PDPSim.h
deleted file mode 100644
index 72d8233..0000000
--- a/hal/src/main/native/include/simulation/PDPSim.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/PDPData.h"
-
-namespace frc {
-namespace sim {
-class PDPSim {
- public:
-  explicit PDPSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPDPInitializedCallback);
-    store->SetUid(HALSIM_RegisterPDPInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const { return HALSIM_GetPDPInitialized(m_index); }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetPDPInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterTemperatureCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPDPTemperatureCallback);
-    store->SetUid(HALSIM_RegisterPDPTemperatureCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetTemperature() const { return HALSIM_GetPDPTemperature(m_index); }
-
-  void SetTemperature(double temperature) {
-    HALSIM_SetPDPTemperature(m_index, temperature);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPDPVoltageCallback);
-    store->SetUid(HALSIM_RegisterPDPVoltageCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetVoltage() const { return HALSIM_GetPDPVoltage(m_index); }
-
-  void SetVoltage(double voltage) { HALSIM_SetPDPVoltage(m_index, voltage); }
-
-  std::unique_ptr<CallbackStore> RegisterCurrentCallback(
-      int channel, NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, channel, -1, callback, &HALSIM_CancelPDPCurrentCallback);
-    store->SetUid(HALSIM_RegisterPDPCurrentCallback(
-        m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetCurrent(int channel) const {
-    return HALSIM_GetPDPCurrent(m_index, channel);
-  }
-
-  void SetCurrent(int channel, double current) {
-    HALSIM_SetPDPCurrent(m_index, channel, current);
-  }
-
-  void GetAllCurrents(double* currents) {
-    HALSIM_GetPDPAllCurrents(m_index, currents);
-  }
-
-  void SetAllCurrents(const double* currents) {
-    HALSIM_SetPDPAllCurrents(m_index, currents);
-  }
-
-  void ResetData() { HALSIM_ResetPDPData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/PWMSim.h

diff --git a/hal/src/main/native/include/simulation/PWMSim.h b/hal/src/main/native/include/simulation/PWMSim.h
deleted file mode 100644
index 7aba360..0000000
--- a/hal/src/main/native/include/simulation/PWMSim.h
+++ /dev/null
@@ -1,114 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/PWMData.h"
-
-namespace frc {
-namespace sim {
-class PWMSim {
- public:
-  explicit PWMSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPWMInitializedCallback);
-    store->SetUid(HALSIM_RegisterPWMInitializedCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitialized() const { return HALSIM_GetPWMInitialized(m_index); }
-
-  void SetInitialized(bool initialized) {
-    HALSIM_SetPWMInitialized(m_index, initialized);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterRawValueCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPWMRawValueCallback);
-    store->SetUid(HALSIM_RegisterPWMRawValueCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetRawValue() const { return HALSIM_GetPWMRawValue(m_index); }
-
-  void SetRawValue(int rawValue) { HALSIM_SetPWMRawValue(m_index, rawValue); }
-
-  std::unique_ptr<CallbackStore> RegisterSpeedCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPWMSpeedCallback);
-    store->SetUid(HALSIM_RegisterPWMSpeedCallback(m_index, &CallbackStoreThunk,
-                                                  store.get(), initialNotify));
-    return store;
-  }
-
-  double GetSpeed() const { return HALSIM_GetPWMSpeed(m_index); }
-
-  void SetSpeed(double speed) { HALSIM_SetPWMSpeed(m_index, speed); }
-
-  std::unique_ptr<CallbackStore> RegisterPositionCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPWMPositionCallback);
-    store->SetUid(HALSIM_RegisterPWMPositionCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetPosition() const { return HALSIM_GetPWMPosition(m_index); }
-
-  void SetPosition(double position) {
-    HALSIM_SetPWMPosition(m_index, position);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterPeriodScaleCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPWMPeriodScaleCallback);
-    store->SetUid(HALSIM_RegisterPWMPeriodScaleCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetPeriodScale() const { return HALSIM_GetPWMPeriodScale(m_index); }
-
-  void SetPeriodScale(int periodScale) {
-    HALSIM_SetPWMPeriodScale(m_index, periodScale);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterZeroLatchCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelPWMZeroLatchCallback);
-    store->SetUid(HALSIM_RegisterPWMZeroLatchCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetZeroLatch() const { return HALSIM_GetPWMZeroLatch(m_index); }
-
-  void SetZeroLatch(bool zeroLatch) {
-    HALSIM_SetPWMZeroLatch(m_index, zeroLatch);
-  }
-
-  void ResetData() { HALSIM_ResetPWMData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/RelaySim.h

diff --git a/hal/src/main/native/include/simulation/RelaySim.h b/hal/src/main/native/include/simulation/RelaySim.h
deleted file mode 100644
index 6958a40..0000000
--- a/hal/src/main/native/include/simulation/RelaySim.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/RelayData.h"
-
-namespace frc {
-namespace sim {
-class RelaySim {
- public:
-  explicit RelaySim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedForwardCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRelayInitializedForwardCallback);
-    store->SetUid(HALSIM_RegisterRelayInitializedForwardCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitializedForward() const {
-    return HALSIM_GetRelayInitializedForward(m_index);
-  }
-
-  void SetInitializedForward(bool initializedForward) {
-    HALSIM_SetRelayInitializedForward(m_index, initializedForward);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterInitializedReverseCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRelayInitializedReverseCallback);
-    store->SetUid(HALSIM_RegisterRelayInitializedReverseCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetInitializedReverse() const {
-    return HALSIM_GetRelayInitializedReverse(m_index);
-  }
-
-  void SetInitializedReverse(bool initializedReverse) {
-    HALSIM_SetRelayInitializedReverse(m_index, initializedReverse);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterForwardCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRelayForwardCallback);
-    store->SetUid(HALSIM_RegisterRelayForwardCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetForward() const { return HALSIM_GetRelayForward(m_index); }
-
-  void SetForward(bool forward) { HALSIM_SetRelayForward(m_index, forward); }
-
-  std::unique_ptr<CallbackStore> RegisterReverseCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRelayReverseCallback);
-    store->SetUid(HALSIM_RegisterRelayReverseCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetReverse() const { return HALSIM_GetRelayReverse(m_index); }
-
-  void SetReverse(bool reverse) { HALSIM_SetRelayReverse(m_index, reverse); }
-
-  void ResetData() { HALSIM_ResetRelayData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/RoboRioSim.h

diff --git a/hal/src/main/native/include/simulation/RoboRioSim.h b/hal/src/main/native/include/simulation/RoboRioSim.h
deleted file mode 100644
index 9b020ae..0000000
--- a/hal/src/main/native/include/simulation/RoboRioSim.h
+++ /dev/null
@@ -1,273 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/RoboRioData.h"
-
-namespace frc {
-namespace sim {
-class RoboRioSim {
- public:
-  explicit RoboRioSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterFPGAButtonCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioFPGAButtonCallback);
-    store->SetUid(HALSIM_RegisterRoboRioFPGAButtonCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetFPGAButton() const { return HALSIM_GetRoboRioFPGAButton(m_index); }
-
-  void SetFPGAButton(bool fPGAButton) {
-    HALSIM_SetRoboRioFPGAButton(m_index, fPGAButton);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterVInVoltageCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioVInVoltageCallback);
-    store->SetUid(HALSIM_RegisterRoboRioVInVoltageCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetVInVoltage() const { return HALSIM_GetRoboRioVInVoltage(m_index); }
-
-  void SetVInVoltage(double vInVoltage) {
-    HALSIM_SetRoboRioVInVoltage(m_index, vInVoltage);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterVInCurrentCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioVInCurrentCallback);
-    store->SetUid(HALSIM_RegisterRoboRioVInCurrentCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetVInCurrent() const { return HALSIM_GetRoboRioVInCurrent(m_index); }
-
-  void SetVInCurrent(double vInCurrent) {
-    HALSIM_SetRoboRioVInCurrent(m_index, vInCurrent);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserVoltage6VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserVoltage6VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserVoltage6VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetUserVoltage6V() const {
-    return HALSIM_GetRoboRioUserVoltage6V(m_index);
-  }
-
-  void SetUserVoltage6V(double userVoltage6V) {
-    HALSIM_SetRoboRioUserVoltage6V(m_index, userVoltage6V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserCurrent6VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserCurrent6VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserCurrent6VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetUserCurrent6V() const {
-    return HALSIM_GetRoboRioUserCurrent6V(m_index);
-  }
-
-  void SetUserCurrent6V(double userCurrent6V) {
-    HALSIM_SetRoboRioUserCurrent6V(m_index, userCurrent6V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserActive6VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserActive6VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserActive6VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetUserActive6V() const {
-    return HALSIM_GetRoboRioUserActive6V(m_index);
-  }
-
-  void SetUserActive6V(bool userActive6V) {
-    HALSIM_SetRoboRioUserActive6V(m_index, userActive6V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserVoltage5VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserVoltage5VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserVoltage5VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetUserVoltage5V() const {
-    return HALSIM_GetRoboRioUserVoltage5V(m_index);
-  }
-
-  void SetUserVoltage5V(double userVoltage5V) {
-    HALSIM_SetRoboRioUserVoltage5V(m_index, userVoltage5V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserCurrent5VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserCurrent5VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserCurrent5VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetUserCurrent5V() const {
-    return HALSIM_GetRoboRioUserCurrent5V(m_index);
-  }
-
-  void SetUserCurrent5V(double userCurrent5V) {
-    HALSIM_SetRoboRioUserCurrent5V(m_index, userCurrent5V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserActive5VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserActive5VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserActive5VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetUserActive5V() const {
-    return HALSIM_GetRoboRioUserActive5V(m_index);
-  }
-
-  void SetUserActive5V(bool userActive5V) {
-    HALSIM_SetRoboRioUserActive5V(m_index, userActive5V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserVoltage3V3Callback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserVoltage3V3Callback);
-    store->SetUid(HALSIM_RegisterRoboRioUserVoltage3V3Callback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetUserVoltage3V3() const {
-    return HALSIM_GetRoboRioUserVoltage3V3(m_index);
-  }
-
-  void SetUserVoltage3V3(double userVoltage3V3) {
-    HALSIM_SetRoboRioUserVoltage3V3(m_index, userVoltage3V3);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserCurrent3V3Callback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserCurrent3V3Callback);
-    store->SetUid(HALSIM_RegisterRoboRioUserCurrent3V3Callback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetUserCurrent3V3() const {
-    return HALSIM_GetRoboRioUserCurrent3V3(m_index);
-  }
-
-  void SetUserCurrent3V3(double userCurrent3V3) {
-    HALSIM_SetRoboRioUserCurrent3V3(m_index, userCurrent3V3);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserActive3V3Callback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserActive3V3Callback);
-    store->SetUid(HALSIM_RegisterRoboRioUserActive3V3Callback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetUserActive3V3() const {
-    return HALSIM_GetRoboRioUserActive3V3(m_index);
-  }
-
-  void SetUserActive3V3(bool userActive3V3) {
-    HALSIM_SetRoboRioUserActive3V3(m_index, userActive3V3);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserFaults6VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserFaults6VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserFaults6VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetUserFaults6V() const { return HALSIM_GetRoboRioUserFaults6V(m_index); }
-
-  void SetUserFaults6V(int userFaults6V) {
-    HALSIM_SetRoboRioUserFaults6V(m_index, userFaults6V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserFaults5VCallback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserFaults5VCallback);
-    store->SetUid(HALSIM_RegisterRoboRioUserFaults5VCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetUserFaults5V() const { return HALSIM_GetRoboRioUserFaults5V(m_index); }
-
-  void SetUserFaults5V(int userFaults5V) {
-    HALSIM_SetRoboRioUserFaults5V(m_index, userFaults5V);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterUserFaults3V3Callback(
-      NotifyCallback callback, bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelRoboRioUserFaults3V3Callback);
-    store->SetUid(HALSIM_RegisterRoboRioUserFaults3V3Callback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetUserFaults3V3() const {
-    return HALSIM_GetRoboRioUserFaults3V3(m_index);
-  }
-
-  void SetUserFaults3V3(int userFaults3V3) {
-    HALSIM_SetRoboRioUserFaults3V3(m_index, userFaults3V3);
-  }
-
-  void ResetData() { HALSIM_ResetRoboRioData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/SPIAccelerometerSim.h

diff --git a/hal/src/main/native/include/simulation/SPIAccelerometerSim.h b/hal/src/main/native/include/simulation/SPIAccelerometerSim.h
deleted file mode 100644
index d7795ee..0000000
--- a/hal/src/main/native/include/simulation/SPIAccelerometerSim.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-
-#include "CallbackStore.h"
-#include "mockdata/SPIAccelerometerData.h"
-
-namespace frc {
-namespace sim {
-class SPIAccelerometerSim {
- public:
-  explicit SPIAccelerometerSim(int index) { m_index = index; }
-
-  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
-                                                        bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelSPIAccelerometerActiveCallback);
-    store->SetUid(HALSIM_RegisterSPIAccelerometerActiveCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  bool GetActive() const { return HALSIM_GetSPIAccelerometerActive(m_index); }
-
-  void SetActive(bool active) {
-    HALSIM_SetSPIAccelerometerActive(m_index, active);
-  }
-
-  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
-                                                       bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelSPIAccelerometerRangeCallback);
-    store->SetUid(HALSIM_RegisterSPIAccelerometerRangeCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  int GetRange() const { return HALSIM_GetSPIAccelerometerRange(m_index); }
-
-  void SetRange(int range) { HALSIM_SetSPIAccelerometerRange(m_index, range); }
-
-  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
-                                                   bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelSPIAccelerometerXCallback);
-    store->SetUid(HALSIM_RegisterSPIAccelerometerXCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetX() const { return HALSIM_GetSPIAccelerometerX(m_index); }
-
-  void SetX(double x) { HALSIM_SetSPIAccelerometerX(m_index, x); }
-
-  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
-                                                   bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelSPIAccelerometerYCallback);
-    store->SetUid(HALSIM_RegisterSPIAccelerometerYCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetY() const { return HALSIM_GetSPIAccelerometerY(m_index); }
-
-  void SetY(double y) { HALSIM_SetSPIAccelerometerY(m_index, y); }
-
-  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
-                                                   bool initialNotify) {
-    auto store = std::make_unique<CallbackStore>(
-        m_index, -1, callback, &HALSIM_CancelSPIAccelerometerZCallback);
-    store->SetUid(HALSIM_RegisterSPIAccelerometerZCallback(
-        m_index, &CallbackStoreThunk, store.get(), initialNotify));
-    return store;
-  }
-
-  double GetZ() const { return HALSIM_GetSPIAccelerometerZ(m_index); }
-
-  void SetZ(double z) { HALSIM_SetSPIAccelerometerZ(m_index, z); }
-
-  void ResetData() { HALSIM_ResetSPIAccelerometerData(m_index); }
-
- private:
-  int m_index;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/SimDeviceSim.h

diff --git a/hal/src/main/native/include/simulation/SimDeviceSim.h b/hal/src/main/native/include/simulation/SimDeviceSim.h
deleted file mode 100644
index 5705a08..0000000
--- a/hal/src/main/native/include/simulation/SimDeviceSim.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-#include <memory>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "CallbackStore.h"
-#include "hal/SimDevice.h"
-#include "mockdata/SimDeviceData.h"
-
-namespace frc {
-namespace sim {
-class SimDeviceSim {
- public:
-  explicit SimDeviceSim(const char* name)
-      : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
-
-  hal::SimValue GetValue(const char* name) const {
-    return HALSIM_GetSimValueHandle(m_handle, name);
-  }
-
-  hal::SimDouble GetDouble(const char* name) const {
-    return HALSIM_GetSimValueHandle(m_handle, name);
-  }
-
-  hal::SimEnum GetEnum(const char* name) const {
-    return HALSIM_GetSimValueHandle(m_handle, name);
-  }
-
-  hal::SimBoolean GetBoolean(const char* name) const {
-    return HALSIM_GetSimValueHandle(m_handle, name);
-  }
-
-  static std::vector<std::string> GetEnumOptions(hal::SimEnum val) {
-    int32_t numOptions;
-    const char** options = HALSIM_GetSimValueEnumOptions(val, &numOptions);
-    std::vector<std::string> rv;
-    rv.reserve(numOptions);
-    for (int32_t i = 0; i < numOptions; ++i) rv.emplace_back(options[i]);
-    return rv;
-  }
-
-  template <typename F>
-  void EnumerateValues(F callback) const {
-    return HALSIM_EnumerateSimValues(
-        m_handle, &callback,
-        [](const char* name, void* param, HAL_SimValueHandle handle,
-           HAL_Bool readonly, const struct HAL_Value* value) {
-          std::invoke(*static_cast<F*>(param), name, handle, readonly, value);
-        });
-  }
-
-  operator HAL_SimDeviceHandle() const { return m_handle; }
-
-  template <typename F>
-  static void EnumerateDevices(const char* prefix, F callback) {
-    return HALSIM_EnumerateSimDevices(
-        prefix, &callback,
-        [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-          std::invoke(*static_cast<F*>(param), name, handle);
-        });
-  }
-
-  static void ResetData() { HALSIM_ResetSimDeviceData(); }
-
- private:
-  HAL_SimDeviceHandle m_handle;
-};
-}  // namespace sim
-}  // namespace frc

hal/src/main/native/include/simulation/SimHooks.h

diff --git a/hal/src/main/native/include/simulation/SimHooks.h b/hal/src/main/native/include/simulation/SimHooks.h
deleted file mode 100644
index 537cef3..0000000
--- a/hal/src/main/native/include/simulation/SimHooks.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "mockdata/MockHooks.h"
-
-namespace frc {
-namespace sim {
-
-void WaitForProgramStart() { HALSIM_WaitForProgramStart(); }
-
-void SetProgramStarted() { HALSIM_SetProgramStarted(); }
-
-bool GetProgramStarted() { return HALSIM_GetProgramStarted(); }
-
-void RestartTiming() { HALSIM_RestartTiming(); }
-
-}  // namespace sim
-}  // namespace frc

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

diff --git a/hal/src/main/native/sim/AnalogGyro.cpp b/hal/src/main/native/sim/AnalogGyro.cpp
index b7d84d9..9666f4a 100644
--- a/hal/src/main/native/sim/AnalogGyro.cpp
+++ b/hal/src/main/native/sim/AnalogGyro.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,13 +7,10 @@
 
 #include "hal/AnalogGyro.h"
 
-#include <chrono>
-#include <thread>
-
-#include "AnalogInternal.h"
 #include "HALInitializer.h"
+#include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
-#include "hal/AnalogInput.h"
+#include "hal/Errors.h"
 #include "hal/handles/IndexedHandleResource.h"
 #include "mockdata/AnalogGyroDataInternal.h"
 

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

diff --git a/hal/src/main/native/sim/AnalogInternal.cpp b/hal/src/main/native/sim/AnalogInternal.cpp
index 1e6a755..820dc97 100644
--- a/hal/src/main/native/sim/AnalogInternal.cpp
+++ b/hal/src/main/native/sim/AnalogInternal.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,7 @@
 #include "AnalogInternal.h"
 
 #include "PortsInternal.h"
-#include "hal/AnalogInput.h"
+#include "hal/handles/IndexedHandleResource.h"
 
 namespace hal {
 IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort, kNumAnalogInputs,

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

diff --git a/hal/src/main/native/sim/AnalogInternal.h b/hal/src/main/native/sim/AnalogInternal.h
index bcc5c95..f4633f9 100644
--- a/hal/src/main/native/sim/AnalogInternal.h
+++ b/hal/src/main/native/sim/AnalogInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,10 +9,8 @@
 
 #include <stdint.h>
 
-#include <memory>
-
 #include "PortsInternal.h"
-#include "hal/Ports.h"
+#include "hal/handles/HandlesInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
 
 namespace hal {

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

diff --git a/hal/src/main/native/sim/AnalogTrigger.cpp b/hal/src/main/native/sim/AnalogTrigger.cpp
index 3ddacee..62bfebc 100644
--- a/hal/src/main/native/sim/AnalogTrigger.cpp
+++ b/hal/src/main/native/sim/AnalogTrigger.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -85,6 +85,7 @@
   trigger->index = static_cast<uint8_t>(getHandleIndex(handle));
 
   SimAnalogTriggerData[trigger->index].initialized = true;
+  SimAnalogTriggerData[trigger->index].inputPort = analog_port->channel;
 
   trigger->trigState = false;
 

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

diff --git a/hal/src/main/native/sim/CANAPI.cpp b/hal/src/main/native/sim/CANAPI.cpp
index 08b0989..c410dea 100644
--- a/hal/src/main/native/sim/CANAPI.cpp
+++ b/hal/src/main/native/sim/CANAPI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,16 +7,13 @@
 
 #include "hal/CANAPI.h"
 
-#include <atomic>
-#include <ctime>
-
 #include <wpi/DenseMap.h>
 
 #include "CANAPIInternal.h"
 #include "HALInitializer.h"
 #include "hal/CAN.h"
 #include "hal/Errors.h"
-#include "hal/HAL.h"
+#include "hal/HALBase.h"
 #include "hal/handles/UnlimitedHandleResource.h"
 
 using namespace hal;

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

diff --git a/hal/src/main/native/sim/CallbackStore.cpp b/hal/src/main/native/sim/CallbackStore.cpp
deleted file mode 100644
index d278b93..0000000
--- a/hal/src/main/native/sim/CallbackStore.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "simulation/CallbackStore.h"
-
-void frc::sim::CallbackStoreThunk(const char* name, void* param,
-                                  const HAL_Value* value) {
-  reinterpret_cast<CallbackStore*>(param)->callback(name, value);
-}

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

diff --git a/hal/src/main/native/sim/Counter.cpp b/hal/src/main/native/sim/Counter.cpp
index 37454d0..f0ea1c4 100644
--- a/hal/src/main/native/sim/Counter.cpp
+++ b/hal/src/main/native/sim/Counter.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 #include "CounterInternal.h"
 #include "HALInitializer.h"
 #include "PortsInternal.h"
-#include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
 

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

diff --git a/hal/src/main/native/sim/DIO.cpp b/hal/src/main/native/sim/DIO.cpp
index 2158136..f6347d9 100644
--- a/hal/src/main/native/sim/DIO.cpp
+++ b/hal/src/main/native/sim/DIO.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,6 @@
 
 #include "hal/DIO.h"
 
-#include <cmath>
-
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
 #include "PortsInternal.h"

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

diff --git a/hal/src/main/native/sim/DigitalInternal.cpp b/hal/src/main/native/sim/DigitalInternal.cpp
index 070754a..db6455a 100644
--- a/hal/src/main/native/sim/DigitalInternal.cpp
+++ b/hal/src/main/native/sim/DigitalInternal.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,11 +7,11 @@
 
 #include "DigitalInternal.h"
 
-#include "ConstantsInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogTrigger.h"
-#include "hal/HAL.h"
-#include "hal/Ports.h"
+#include "hal/Errors.h"
+#include "hal/handles/DigitalHandleResource.h"
+#include "hal/handles/HandlesInternal.h"
 
 namespace hal {
 

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

diff --git a/hal/src/main/native/sim/DigitalInternal.h b/hal/src/main/native/sim/DigitalInternal.h
index 9df4c51..d169c8a 100644
--- a/hal/src/main/native/sim/DigitalInternal.h
+++ b/hal/src/main/native/sim/DigitalInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,14 +9,9 @@
 
 #include <stdint.h>
 
-#include <memory>
-
 #include "PortsInternal.h"
 #include "hal/AnalogTrigger.h"
-#include "hal/Ports.h"
-#include "hal/Types.h"
 #include "hal/handles/DigitalHandleResource.h"
-#include "hal/handles/HandlesInternal.h"
 
 namespace hal {
 /**

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

diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp
index e8404cf..aef8556 100644
--- a/hal/src/main/native/sim/DriverStation.cpp
+++ b/hal/src/main/native/sim/DriverStation.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,10 +18,12 @@
 
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
+#include <wpi/raw_ostream.h>
 
 #include "HALInitializer.h"
+#include "hal/cpp/fpga_clock.h"
+#include "hal/simulation/MockHooks.h"
 #include "mockdata/DriverStationDataInternal.h"
-#include "mockdata/MockHooks.h"
 
 static wpi::mutex msgMutex;
 static wpi::condition_variable* newDSDataAvailableCond;
@@ -29,6 +31,8 @@
 static int newDSDataAvailableCounter{0};
 static std::atomic_bool isFinalized{false};
 static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
+static std::atomic<HALSIM_SendConsoleLineHandler> sendConsoleLineHandler{
+    nullptr};
 
 namespace hal {
 namespace init {
@@ -47,6 +51,10 @@
   sendErrorHandler.store(handler);
 }
 
+void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {
+  sendConsoleLineHandler.store(handler);
+}
+
 int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
                       const char* details, const char* location,
                       const char* callStack, HAL_Bool printMsg) {
@@ -59,18 +67,16 @@
   static constexpr int KEEP_MSGS = 5;
   std::scoped_lock lock(msgMutex);
   static std::string prevMsg[KEEP_MSGS];
-  static std::chrono::time_point<std::chrono::steady_clock>
-      prevMsgTime[KEEP_MSGS];
+  static fpga_clock::time_point prevMsgTime[KEEP_MSGS];
   static bool initialized = false;
   if (!initialized) {
     for (int i = 0; i < KEEP_MSGS; i++) {
-      prevMsgTime[i] =
-          std::chrono::steady_clock::now() - std::chrono::seconds(2);
+      prevMsgTime[i] = fpga_clock::now() - std::chrono::seconds(2);
     }
     initialized = true;
   }
 
-  auto curTime = std::chrono::steady_clock::now();
+  auto curTime = fpga_clock::now();
   int i;
   for (i = 0; i < KEEP_MSGS; ++i) {
     if (prevMsg[i] == details) break;
@@ -105,6 +111,16 @@
   return retval;
 }
 
+int32_t HAL_SendConsoleLine(const char* line) {
+  auto handler = sendConsoleLineHandler.load();
+  if (handler) {
+    return handler(line);
+  }
+  wpi::outs() << line << "\n";
+  wpi::outs().flush();
+  return 0;
+}
+
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   controlWord->enabled = SimDriverStationData->enabled;
   controlWord->autonomous = SimDriverStationData->autonomous;
@@ -201,38 +217,29 @@
   // TODO
 }
 
-#ifdef __APPLE__
-static pthread_key_t lastCountKey;
-static pthread_once_t lastCountKeyOnce = PTHREAD_ONCE_INIT;
-
-static void InitLastCountKey(void) {
-  pthread_key_create(&lastCountKey, std::free);
-}
-#endif
-
 static int& GetThreadLocalLastCount() {
   // There is a rollover error condition here. At Packet# = n * (uintmax), this
   // will return false when instead it should return true. However, this at a
   // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
   // worth the cycles to check.
-#ifdef __APPLE__
-  pthread_once(&lastCountKeyOnce, InitLastCountKey);
-  int* lastCountPtr = static_cast<int*>(pthread_getspecific(lastCountKey));
-  if (!lastCountPtr) {
-    lastCountPtr = static_cast<int*>(std::malloc(sizeof(int)));
-    *lastCountPtr = -1;
-    pthread_setspecific(lastCountKey, lastCountPtr);
-  }
-  int& lastCount = *lastCountPtr;
-#else
-  thread_local int lastCount{-1};
-#endif
+  thread_local int lastCount{0};
   return lastCount;
 }
 
-HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) {
+HAL_Bool HAL_IsNewControlData(void) {
+  std::scoped_lock lock(newDSDataAvailableMutex);
   int& lastCount = GetThreadLocalLastCount();
+  int currentCount = newDSDataAvailableCounter;
+  if (lastCount == currentCount) return false;
+  lastCount = currentCount;
+  return true;
+}
+
+void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); }
+
+HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
   std::unique_lock lock(newDSDataAvailableMutex);
+  int& lastCount = GetThreadLocalLastCount();
   int currentCount = newDSDataAvailableCounter;
   if (lastCount != currentCount) {
     lastCount = currentCount;
@@ -242,7 +249,6 @@
   if (isFinalized.load()) {
     return false;
   }
-
   auto timeoutTime =
       std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
 
@@ -256,42 +262,7 @@
       newDSDataAvailableCond->wait(lock);
     }
   }
-  return true;
-}
-
-HAL_Bool HAL_IsNewControlData(void) {
-  int& lastCount = GetThreadLocalLastCount();
-  int currentCount = 0;
-  {
-    std::scoped_lock lock(newDSDataAvailableMutex);
-    currentCount = newDSDataAvailableCounter;
-  }
-  if (lastCount == currentCount) return false;
-  lastCount = currentCount;
-  return true;
-}
-
-void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); }
-
-HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
-  if (isFinalized.load()) {
-    return false;
-  }
-  auto timeoutTime =
-      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
-
-  std::unique_lock lock(newDSDataAvailableMutex);
-  int currentCount = newDSDataAvailableCounter;
-  while (newDSDataAvailableCounter == currentCount) {
-    if (timeout > 0) {
-      auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
-      if (timedOut == std::cv_status::timeout) {
-        return false;
-      }
-    } else {
-      newDSDataAvailableCond->wait(lock);
-    }
-  }
+  lastCount = newDSDataAvailableCounter;
   return true;
 }
 
@@ -302,6 +273,7 @@
   // Since we could get other values, require our specific handle
   // to signal our threads
   if (refNum != refNumber) return 0;
+  SimDriverStationData->CallNewDataCallbacks();
   std::scoped_lock lock(newDSDataAvailableMutex);
   // Nofify all threads
   newDSDataAvailableCounter++;

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

diff --git a/hal/src/main/native/sim/Encoder.cpp b/hal/src/main/native/sim/Encoder.cpp
index 36122bf..32416f0 100644
--- a/hal/src/main/native/sim/Encoder.cpp
+++ b/hal/src/main/native/sim/Encoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 #include "CounterInternal.h"
 #include "HALInitializer.h"
 #include "PortsInternal.h"
-#include "hal/Counter.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"

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

diff --git a/hal/src/main/native/sim/Extensions.cpp b/hal/src/main/native/sim/Extensions.cpp
index 951ad10..e638d6a 100644
--- a/hal/src/main/native/sim/Extensions.cpp
+++ b/hal/src/main/native/sim/Extensions.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,12 +7,13 @@
 
 #include "hal/Extensions.h"
 
+#include <vector>
+
 #include <wpi/Path.h>
 #include <wpi/SmallString.h>
 #include <wpi/StringRef.h>
 #include <wpi/raw_ostream.h>
-
-#include "hal/HAL.h"
+#include <wpi/spinlock.h>
 
 #if defined(WIN32) || defined(_WIN32)
 #include <windows.h>
@@ -23,9 +24,10 @@
 #if defined(WIN32) || defined(_WIN32)
 #define DELIM ';'
 #define HTYPE HMODULE
-#define DLOPEN(a) LoadLibrary(a)
+#define DLOPEN(a) LoadLibraryA(a)
 #define DLSYM GetProcAddress
 #define DLCLOSE FreeLibrary
+#define DLERROR "error #" << GetLastError()
 #else
 #define DELIM ':'
 #define HTYPE void*
@@ -33,8 +35,14 @@
 #define DLOPEN(a) dlopen(a, RTLD_LAZY)
 #define DLSYM dlsym
 #define DLCLOSE dlclose
+#define DLERROR dlerror()
 #endif
 
+static wpi::recursive_spinlock gExtensionRegistryMutex;
+static std::vector<std::pair<const char*, void*>> gExtensionRegistry;
+static std::vector<std::pair<void*, void (*)(void*, const char*, void*)>>
+    gExtensionListeners;
+
 namespace hal {
 namespace init {
 void InitializeExtensions() {}
@@ -63,14 +71,16 @@
 #else
     libraryName += ".so";
 #endif
-    wpi::outs() << "HAL Extensions: Trying modified name: "
-                << wpi::sys::path::stem(libraryName);
+    wpi::outs() << "HAL Extensions: Load failed: " << DLERROR
+                << "\nTrying modified name: "
+                << wpi::sys::path::stem(libraryName) << "\n";
     wpi::outs().flush();
     handle = DLOPEN(libraryName.c_str());
   }
 #endif
   if (!handle) {
-    wpi::outs() << "HAL Extensions: Failed to load library\n";
+    wpi::outs() << "HAL Extensions: Failed to load library: " << DLERROR
+                << '\n';
     wpi::outs().flush();
     return rc;
   }
@@ -112,6 +122,22 @@
   return rc;
 }
 
+void HAL_RegisterExtension(const char* name, void* data) {
+  std::scoped_lock lock(gExtensionRegistryMutex);
+  gExtensionRegistry.emplace_back(name, data);
+  for (auto&& listener : gExtensionListeners)
+    listener.second(listener.first, name, data);
+}
+
+void HAL_RegisterExtensionListener(void* param,
+                                   void (*func)(void*, const char* name,
+                                                void* data)) {
+  std::scoped_lock lock(gExtensionRegistryMutex);
+  gExtensionListeners.emplace_back(param, func);
+  for (auto&& extension : gExtensionRegistry)
+    func(param, extension.first, extension.second);
+}
+
 void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage) {
   GetShowNotFoundMessage() = showMessage;
 }

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

diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp
index 0dda617..64c4a10 100644
--- a/hal/src/main/native/sim/HAL.cpp
+++ b/hal/src/main/native/sim/HAL.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,16 @@
 
 #include "hal/HAL.h"
 
+#include <vector>
+
 #include <wpi/mutex.h>
 #include <wpi/raw_ostream.h>
+#include <wpi/spinlock.h>
+
+#ifdef _WIN32
+#include <Windows.h>
+#pragma comment(lib, "Winmm.lib")
+#endif  // _WIN32
 
 #include "ErrorsInternal.h"
 #include "HALInitializer.h"
@@ -17,10 +25,15 @@
 #include "hal/Errors.h"
 #include "hal/Extensions.h"
 #include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/DriverStationData.h"
 #include "mockdata/RoboRioDataInternal.h"
 
 using namespace hal;
 
+static HAL_RuntimeType runtimeType{HAL_Mock};
+static wpi::spinlock gOnShutdownMutex;
+static std::vector<std::pair<void*, void (*)(void*)>> gOnShutdown;
+
 namespace hal {
 namespace init {
 void InitializeHAL() {
@@ -53,6 +66,7 @@
   InitializeAnalogInput();
   InitializeAnalogInternal();
   InitializeAnalogOutput();
+  InitializeAnalogTrigger();
   InitializeCAN();
   InitializeCompressor();
   InitializeConstants();
@@ -214,7 +228,9 @@
   }
 }
 
-HAL_RuntimeType HAL_GetRuntimeType(void) { return HAL_Mock; }
+HAL_RuntimeType HAL_GetRuntimeType(void) { return runtimeType; }
+
+void HALSIM_SetRuntimeType(HAL_RuntimeType type) { runtimeType = type; }
 
 int32_t HAL_GetFPGAVersion(int32_t* status) {
   return 2018;  // Automatically script this at some point
@@ -253,7 +269,7 @@
 }
 
 HAL_Bool HAL_GetSystemActive(int32_t* status) {
-  return true;  // Figure out if we need to handle this
+  return HALSIM_GetDriverStationEnabled();
 }
 
 HAL_Bool HAL_GetBrownedOut(int32_t* status) {
@@ -274,15 +290,49 @@
 
   hal::init::HAL_IsInitialized.store(true);
 
-  wpi::outs().SetUnbuffered();
-  if (HAL_LoadExtensions() < 0) return false;
   hal::RestartTiming();
   HAL_InitializeDriverStation();
 
   initialized = true;
+
+// Set Timer Precision to 1ms on Windows
+#ifdef _WIN32
+  TIMECAPS tc;
+  if (timeGetDevCaps(&tc, sizeof(tc)) == TIMERR_NOERROR) {
+    UINT target = min(1, tc.wPeriodMin);
+    timeBeginPeriod(target);
+    std::atexit([]() {
+      TIMECAPS tc;
+      if (timeGetDevCaps(&tc, sizeof(tc)) == TIMERR_NOERROR) {
+        UINT target = min(1, tc.wPeriodMin);
+        timeEndPeriod(target);
+      }
+    });
+  }
+#endif  // _WIN32
+
+  wpi::outs().SetUnbuffered();
+  if (HAL_LoadExtensions() < 0) return false;
+
   return true;  // Add initialization if we need to at a later point
 }
 
+void HAL_Shutdown(void) {
+  std::vector<std::pair<void*, void (*)(void*)>> funcs;
+  {
+    std::scoped_lock lock(gOnShutdownMutex);
+    funcs.swap(gOnShutdown);
+  }
+  for (auto&& func : funcs) {
+    func.second(func.first);
+  }
+}
+
+void HAL_OnShutdown(void* param, void (*func)(void*)) {
+  std::scoped_lock lock(gOnShutdownMutex);
+  gOnShutdown.emplace_back(param, func);
+}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   return 0;  // Do nothing for now

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

diff --git a/hal/src/main/native/sim/HALInitializer.cpp b/hal/src/main/native/sim/HALInitializer.cpp
index a0456d4..5c2242b 100644
--- a/hal/src/main/native/sim/HALInitializer.cpp
+++ b/hal/src/main/native/sim/HALInitializer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,7 @@
 
 #include "HALInitializer.h"
 
-#include "hal/HAL.h"
+#include "hal/HALBase.h"
 
 namespace hal {
 namespace init {

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

diff --git a/hal/src/main/native/sim/HALInitializer.h b/hal/src/main/native/sim/HALInitializer.h
index c08df73..29efac5 100644
--- a/hal/src/main/native/sim/HALInitializer.h
+++ b/hal/src/main/native/sim/HALInitializer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -48,6 +48,7 @@
 extern void InitializeAnalogInput();
 extern void InitializeAnalogInternal();
 extern void InitializeAnalogOutput();
+extern void InitializeAnalogTrigger();
 extern void InitializeCAN();
 extern void InitializeCompressor();
 extern void InitializeConstants();

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

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

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

diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp
index 59086ae..8fe387f 100644
--- a/hal/src/main/native/sim/MockHooks.cpp
+++ b/hal/src/main/native/sim/MockHooks.cpp
@@ -1,10 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <algorithm>
 #include <atomic>
 #include <chrono>
 #include <cstdio>
@@ -14,45 +15,46 @@
 
 #include "MockHooksInternal.h"
 #include "NotifierInternal.h"
+#include "hal/simulation/NotifierData.h"
 
 static std::atomic<bool> programStarted{false};
 
 static std::atomic<uint64_t> programStartTime{0};
 static std::atomic<uint64_t> programPauseTime{0};
+static std::atomic<uint64_t> programStepTime{0};
 
 namespace hal {
 namespace init {
-void InitializeMockHooks() {}
+void InitializeMockHooks() { wpi::SetNowImpl(GetFPGATime); }
 }  // namespace init
 }  // namespace hal
 
 namespace hal {
 void RestartTiming() {
-  programStartTime = wpi::Now();
+  programStartTime = wpi::NowDefault();
+  programStepTime = 0;
   if (programPauseTime != 0) programPauseTime = programStartTime.load();
 }
 
 void PauseTiming() {
-  if (programPauseTime == 0) programPauseTime = wpi::Now();
+  if (programPauseTime == 0) programPauseTime = wpi::NowDefault();
 }
 
 void ResumeTiming() {
   if (programPauseTime != 0) {
-    programStartTime += wpi::Now() - programPauseTime;
+    programStartTime += wpi::NowDefault() - programPauseTime;
     programPauseTime = 0;
   }
 }
 
 bool IsTimingPaused() { return programPauseTime != 0; }
 
-void StepTiming(uint64_t delta) {
-  if (programPauseTime != 0) programPauseTime += delta;
-}
+void StepTiming(uint64_t delta) { programStepTime += delta; }
 
-int64_t GetFPGATime() {
+uint64_t GetFPGATime() {
   uint64_t curTime = programPauseTime;
-  if (curTime == 0) curTime = wpi::Now();
-  return curTime - programStartTime;
+  if (curTime == 0) curTime = wpi::NowDefault();
+  return curTime + programStepTime - programStartTime;
 }
 
 double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; }
@@ -92,6 +94,22 @@
 HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
 
 void HALSIM_StepTiming(uint64_t delta) {
+  WaitNotifiers();
+
+  while (delta > 0) {
+    int32_t status = 0;
+    uint64_t curTime = HAL_GetFPGATime(&status);
+    uint64_t nextTimeout = HALSIM_GetNextNotifierTimeout();
+    uint64_t step = std::min(delta, nextTimeout - curTime);
+
+    StepTiming(step);
+    delta -= step;
+
+    WakeupWaitNotifiers();
+  }
+}
+
+void HALSIM_StepTimingAsync(uint64_t delta) {
   StepTiming(delta);
   WakeupNotifiers();
 }

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

diff --git a/hal/src/main/native/sim/MockHooksInternal.h b/hal/src/main/native/sim/MockHooksInternal.h
index a69e9bf..c53f120 100644
--- a/hal/src/main/native/sim/MockHooksInternal.h
+++ b/hal/src/main/native/sim/MockHooksInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <stdint.h>
 
-#include "mockdata/MockHooks.h"
+#include "hal/simulation/MockHooks.h"
 
 namespace hal {
 void RestartTiming();
@@ -22,7 +22,7 @@
 
 void StepTiming(uint64_t delta);
 
-int64_t GetFPGATime();
+uint64_t GetFPGATime();
 
 double GetFPGATimestamp();
 

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

diff --git a/hal/src/main/native/sim/Notifier.cpp b/hal/src/main/native/sim/Notifier.cpp
index 211f7b2..a697275 100644
--- a/hal/src/main/native/sim/Notifier.cpp
+++ b/hal/src/main/native/sim/Notifier.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,23 +13,26 @@
 #include <cstring>
 #include <string>
 
+#include <wpi/SmallVector.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
-#include <wpi/timestamp.h>
 
 #include "HALInitializer.h"
 #include "NotifierInternal.h"
-#include "hal/HAL.h"
+#include "hal/Errors.h"
+#include "hal/HALBase.h"
 #include "hal/cpp/fpga_clock.h"
 #include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifierData.h"
+#include "hal/simulation/NotifierData.h"
 
 namespace {
 struct Notifier {
   std::string name;
-  uint64_t waitTime;
+  uint64_t waitTime = UINT64_MAX;
   bool active = true;
-  bool running = false;
+  bool waitTimeValid = false;    // True if waitTime is set and in the future
+  bool waitingForAlarm = false;  // True if in HAL_WaitForNotifierAlarm()
+  uint64_t waitCount = 0;        // Counts calls to HAL_WaitForNotifierAlarm()
   wpi::mutex mutex;
   wpi::condition_variable cond;
 };
@@ -37,6 +40,9 @@
 
 using namespace hal;
 
+static wpi::mutex notifiersWaiterMutex;
+static wpi::condition_variable notifiersWaiterCond;
+
 class NotifierHandleContainer
     : public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
                                      HAL_HandleEnum::Notifier> {
@@ -46,10 +52,11 @@
       {
         std::scoped_lock lock(notifier->mutex);
         notifier->active = false;
-        notifier->running = false;
+        notifier->waitTimeValid = false;
       }
       notifier->cond.notify_all();  // wake up any waiting threads
     });
+    notifiersWaiterCond.notify_all();
   }
 };
 
@@ -76,6 +83,79 @@
     notifier->cond.notify_all();
   });
 }
+
+void WaitNotifiers() {
+  std::unique_lock ulock(notifiersWaiterMutex);
+  wpi::SmallVector<HAL_NotifierHandle, 8> waiters;
+
+  // Wait for all Notifiers to hit HAL_WaitForNotifierAlarm()
+  notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
+    std::scoped_lock lock(notifier->mutex);
+    if (notifier->active && !notifier->waitingForAlarm) {
+      waiters.emplace_back(handle);
+    }
+  });
+  for (;;) {
+    int count = 0;
+    int end = waiters.size();
+    while (count < end) {
+      auto& it = waiters[count];
+      if (auto notifier = notifierHandles->Get(it)) {
+        std::scoped_lock lock(notifier->mutex);
+        if (notifier->active && !notifier->waitingForAlarm) {
+          ++count;
+          continue;
+        }
+      }
+      // No longer need to wait for it, put at end so it can be erased
+      std::swap(it, waiters[--end]);
+    }
+    if (count == 0) break;
+    waiters.resize(count);
+    notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
+  }
+}
+
+void WakeupWaitNotifiers() {
+  std::unique_lock ulock(notifiersWaiterMutex);
+  int32_t status = 0;
+  uint64_t curTime = HAL_GetFPGATime(&status);
+  wpi::SmallVector<std::pair<HAL_NotifierHandle, uint64_t>, 8> waiters;
+
+  // Wake up Notifiers that have expired timeouts
+  notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
+    std::scoped_lock lock(notifier->mutex);
+
+    // Only wait for the Notifier if it has a valid timeout that's expired
+    if (notifier->active && notifier->waitTimeValid &&
+        curTime >= notifier->waitTime) {
+      waiters.emplace_back(handle, notifier->waitCount);
+      notifier->cond.notify_all();
+    }
+  });
+  for (;;) {
+    int count = 0;
+    int end = waiters.size();
+    while (count < end) {
+      auto& it = waiters[count];
+      if (auto notifier = notifierHandles->Get(it.first)) {
+        std::scoped_lock lock(notifier->mutex);
+
+        // waitCount is used here instead of waitingForAlarm because we want to
+        // wait until HAL_WaitForNotifierAlarm() is exited, then reentered
+        if (notifier->active && notifier->waitCount == it.second) {
+          ++count;
+          continue;
+        }
+      }
+      // No longer need to wait for it, put at end so it can be erased
+      it.swap(waiters[--end]);
+    }
+    if (count == 0) break;
+    waiters.resize(count);
+    notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
+  }
+}
 }  // namespace hal
 
 extern "C" {
@@ -106,7 +186,7 @@
   {
     std::scoped_lock lock(notifier->mutex);
     notifier->active = false;
-    notifier->running = false;
+    notifier->waitTimeValid = false;
   }
   notifier->cond.notify_all();
 }
@@ -119,7 +199,7 @@
   {
     std::scoped_lock lock(notifier->mutex);
     notifier->active = false;
-    notifier->running = false;
+    notifier->waitTimeValid = false;
   }
   notifier->cond.notify_all();
 }
@@ -132,7 +212,7 @@
   {
     std::scoped_lock lock(notifier->mutex);
     notifier->waitTime = triggerTime;
-    notifier->running = true;
+    notifier->waitTimeValid = (triggerTime != UINT64_MAX);
   }
 
   // We wake up any waiters to change how long they're sleeping for
@@ -146,7 +226,7 @@
 
   {
     std::scoped_lock lock(notifier->mutex);
-    notifier->running = false;
+    notifier->waitTimeValid = false;
   }
 }
 
@@ -155,26 +235,31 @@
   auto notifier = notifierHandles->Get(notifierHandle);
   if (!notifier) return 0;
 
+  std::unique_lock ulock(notifiersWaiterMutex);
   std::unique_lock lock(notifier->mutex);
+  notifier->waitingForAlarm = true;
+  ++notifier->waitCount;
+  ulock.unlock();
+  notifiersWaiterCond.notify_all();
   while (notifier->active) {
-    double waitTime;
-    if (!notifier->running || notifiersPaused) {
-      waitTime = (HAL_GetFPGATime(status) * 1e-6) + 1000.0;
-      // If not running, wait 1000 seconds
-    } else {
-      waitTime = notifier->waitTime * 1e-6;
+    uint64_t curTime = HAL_GetFPGATime(status);
+    if (notifier->waitTimeValid && curTime >= notifier->waitTime) {
+      notifier->waitTimeValid = false;
+      notifier->waitingForAlarm = false;
+      return curTime;
     }
 
-    auto timeoutTime =
-        hal::fpga_clock::epoch() + std::chrono::duration<double>(waitTime);
-    notifier->cond.wait_until(lock, timeoutTime);
-    if (!notifier->running) continue;
-    if (!notifier->active) break;
-    uint64_t curTime = HAL_GetFPGATime(status);
-    if (curTime < notifier->waitTime) continue;
-    notifier->running = false;
-    return curTime;
+    double waitDuration;
+    if (!notifier->waitTimeValid || notifiersPaused) {
+      // If not running, wait 1000 seconds
+      waitDuration = 1000.0;
+    } else {
+      waitDuration = (notifier->waitTime - curTime) * 1e-6;
+    }
+
+    notifier->cond.wait_for(lock, std::chrono::duration<double>(waitDuration));
   }
+  notifier->waitingForAlarm = false;
   return 0;
 }
 
@@ -182,7 +267,8 @@
   uint64_t timeout = UINT64_MAX;
   notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
     std::scoped_lock lock(notifier->mutex);
-    if (notifier->active && notifier->running && timeout > notifier->waitTime)
+    if (notifier->active && notifier->waitTimeValid &&
+        timeout > notifier->waitTime)
       timeout = notifier->waitTime;
   });
   return timeout;
@@ -213,7 +299,7 @@
         arr[num].name[sizeof(arr[num].name) - 1] = '\0';
       }
       arr[num].timeout = notifier->waitTime;
-      arr[num].running = notifier->running;
+      arr[num].waitTimeValid = notifier->waitTimeValid;
     }
     ++num;
   });

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

diff --git a/hal/src/main/native/sim/NotifierInternal.h b/hal/src/main/native/sim/NotifierInternal.h
index 84232d2..e6692ca 100644
--- a/hal/src/main/native/sim/NotifierInternal.h
+++ b/hal/src/main/native/sim/NotifierInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,4 +11,6 @@
 void PauseNotifiers();
 void ResumeNotifiers();
 void WakeupNotifiers();
+void WaitNotifiers();
+void WakeupWaitNotifiers();
 }  // namespace hal

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

diff --git a/hal/src/main/native/sim/PDP.cpp b/hal/src/main/native/sim/PDP.cpp
index dbe09d4..cc095d0 100644
--- a/hal/src/main/native/sim/PDP.cpp
+++ b/hal/src/main/native/sim/PDP.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 #include "HALInitializer.h"
 #include "PortsInternal.h"
 #include "hal/CANAPI.h"
-#include "hal/handles/IndexedHandleResource.h"
+#include "hal/Errors.h"
 #include "mockdata/PDPDataInternal.h"
 
 using namespace hal;

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

diff --git a/hal/src/main/native/sim/PortsInternal.h b/hal/src/main/native/sim/PortsInternal.h
index cfbf1e7..d143aca 100644
--- a/hal/src/main/native/sim/PortsInternal.h
+++ b/hal/src/main/native/sim/PortsInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,7 +17,7 @@
 constexpr int32_t kNumCounters = 8;
 constexpr int32_t kNumDigitalHeaders = 10;
 constexpr int32_t kNumPWMHeaders = 10;
-constexpr int32_t kNumDigitalChannels = 26;
+constexpr int32_t kNumDigitalChannels = 31;
 constexpr int32_t kNumPWMChannels = 20;
 constexpr int32_t kNumDigitalPWMOutputs = 6;
 constexpr int32_t kNumEncoders = 8;

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

diff --git a/hal/src/main/native/sim/Power.cpp b/hal/src/main/native/sim/Power.cpp
index 95bb3dd..e8ce710 100644
--- a/hal/src/main/native/sim/Power.cpp
+++ b/hal/src/main/native/sim/Power.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,46 +19,42 @@
 
 // TODO: Fix the naming in here
 extern "C" {
-double HAL_GetVinVoltage(int32_t* status) {
-  return SimRoboRioData[0].vInVoltage;
-}
-double HAL_GetVinCurrent(int32_t* status) {
-  return SimRoboRioData[0].vInCurrent;
-}
+double HAL_GetVinVoltage(int32_t* status) { return SimRoboRioData->vInVoltage; }
+double HAL_GetVinCurrent(int32_t* status) { return SimRoboRioData->vInCurrent; }
 double HAL_GetUserVoltage6V(int32_t* status) {
-  return SimRoboRioData[0].userVoltage6V;
+  return SimRoboRioData->userVoltage6V;
 }
 double HAL_GetUserCurrent6V(int32_t* status) {
-  return SimRoboRioData[0].userCurrent6V;
+  return SimRoboRioData->userCurrent6V;
 }
 HAL_Bool HAL_GetUserActive6V(int32_t* status) {
-  return SimRoboRioData[0].userActive6V;
+  return SimRoboRioData->userActive6V;
 }
 int32_t HAL_GetUserCurrentFaults6V(int32_t* status) {
-  return SimRoboRioData[0].userFaults6V;
+  return SimRoboRioData->userFaults6V;
 }
 double HAL_GetUserVoltage5V(int32_t* status) {
-  return SimRoboRioData[0].userVoltage5V;
+  return SimRoboRioData->userVoltage5V;
 }
 double HAL_GetUserCurrent5V(int32_t* status) {
-  return SimRoboRioData[0].userCurrent5V;
+  return SimRoboRioData->userCurrent5V;
 }
 HAL_Bool HAL_GetUserActive5V(int32_t* status) {
-  return SimRoboRioData[0].userActive5V;
+  return SimRoboRioData->userActive5V;
 }
 int32_t HAL_GetUserCurrentFaults5V(int32_t* status) {
-  return SimRoboRioData[0].userFaults5V;
+  return SimRoboRioData->userFaults5V;
 }
 double HAL_GetUserVoltage3V3(int32_t* status) {
-  return SimRoboRioData[0].userVoltage3V3;
+  return SimRoboRioData->userVoltage3V3;
 }
 double HAL_GetUserCurrent3V3(int32_t* status) {
-  return SimRoboRioData[0].userCurrent3V3;
+  return SimRoboRioData->userCurrent3V3;
 }
 HAL_Bool HAL_GetUserActive3V3(int32_t* status) {
-  return SimRoboRioData[0].userActive3V3;
+  return SimRoboRioData->userActive3V3;
 }
 int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
-  return SimRoboRioData[0].userFaults3V3;
+  return SimRoboRioData->userFaults3V3;
 }
 }  // extern "C"

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

diff --git a/hal/src/main/native/sim/Relay.cpp b/hal/src/main/native/sim/Relay.cpp
index bd1c8a8..acecec4 100644
--- a/hal/src/main/native/sim/Relay.cpp
+++ b/hal/src/main/native/sim/Relay.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -89,8 +89,8 @@
 
 HAL_Bool HAL_CheckRelayChannel(int32_t channel) {
   // roboRIO only has 4 headers, and the FPGA has
-  // seperate functions for forward and reverse,
-  // instead of seperate channel IDs
+  // separate functions for forward and reverse,
+  // instead of separate channel IDs
   return channel < kNumRelayHeaders && channel >= 0;
 }
 

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

diff --git a/hal/src/main/native/sim/Solenoid.cpp b/hal/src/main/native/sim/Solenoid.cpp
index 46bd285..1881a32 100644
--- a/hal/src/main/native/sim/Solenoid.cpp
+++ b/hal/src/main/native/sim/Solenoid.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
-#include "mockdata/PCMDataInternal.h"
+#include "hal/simulation/PCMData.h"
 
 namespace {
 struct Solenoid {

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

diff --git a/hal/src/main/native/sim/jni/AccelerometerDataJNI.cpp b/hal/src/main/native/sim/jni/AccelerometerDataJNI.cpp
deleted file mode 100644
index 8964a41..0000000
--- a/hal/src/main/native/sim/jni/AccelerometerDataJNI.cpp
+++ /dev/null
@@ -1,279 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI.h"
-#include "mockdata/AccelerometerData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    registerActiveCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerActiveCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAccelerometerActiveCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    cancelActiveCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelActiveCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAccelerometerActiveCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    getActive
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getActive
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAccelerometerActive(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    setActive
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setActive
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAccelerometerActive(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    registerRangeCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerRangeCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAccelerometerRangeCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    cancelRangeCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelRangeCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAccelerometerRangeCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    getRange
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getRange
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAccelerometerRange(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    setRange
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setRange
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetAccelerometerRange(index,
-                               static_cast<HAL_AccelerometerRange>(value));
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    registerXCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerXCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAccelerometerXCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    cancelXCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelXCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAccelerometerXCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    getX
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getX
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAccelerometerX(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    setX
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setX
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAccelerometerX(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    registerYCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerYCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAccelerometerYCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    cancelYCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelYCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAccelerometerYCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    getY
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getY
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAccelerometerY(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    setY
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setY
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAccelerometerY(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    registerZCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_registerZCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAccelerometerZCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    cancelZCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_cancelZCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAccelerometerZCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    getZ
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_getZ
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAccelerometerZ(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    setZ
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_setZ
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAccelerometerZ(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AccelerometerDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetAccelerometerData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp b/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp
deleted file mode 100644
index 530eae2..0000000
--- a/hal/src/main/native/sim/jni/AddressableLEDDataJNI.cpp
+++ /dev/null
@@ -1,293 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "ConstBufferCallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI.h"
-#include "mockdata/AddressableLEDData.h"
-
-static_assert(sizeof(jbyte) * 4 == sizeof(HAL_AddressableLEDData));
-
-using namespace wpi::java;
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAddressableLEDInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAddressableLEDInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAddressableLEDInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAddressableLEDInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    registerOutputPortCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerOutputPortCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAddressableLEDOutputPortCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    cancelOutputPortCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelOutputPortCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAddressableLEDOutputPortCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    getOutputPort
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getOutputPort
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAddressableLEDOutputPort(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    setOutputPort
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setOutputPort
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetAddressableLEDOutputPort(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    registerLengthCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerLengthCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAddressableLEDLengthCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    cancelLengthCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelLengthCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAddressableLEDLengthCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    getLength
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getLength
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAddressableLEDLength(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    setLength
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setLength
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetAddressableLEDLength(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    registerRunningCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerRunningCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAddressableLEDRunningCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    cancelRunningCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelRunningCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAddressableLEDRunningCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    getRunning
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getRunning
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAddressableLEDRunning(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    setRunning
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setRunning
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAddressableLEDRunning(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    registerDataCallback
- * Signature: (ILjava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_registerDataCallback
-  (JNIEnv* env, jclass, jint index, jobject callback)
-{
-  return sim::AllocateConstBufferCallback(
-      env, index, callback, &HALSIM_RegisterAddressableLEDDataCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    cancelDataCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_cancelDataCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  sim::FreeConstBufferCallback(env, handle, index,
-                               &HALSIM_CancelAddressableLEDDataCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    getData
- * Signature: (I)[B
- */
-JNIEXPORT jbyteArray JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_getData
-  (JNIEnv* env, jclass, jint index)
-{
-  auto data =
-      std::make_unique<HAL_AddressableLEDData[]>(HAL_kAddressableLEDMaxLength);
-  int32_t length = HALSIM_GetAddressableLEDData(index, data.get());
-  return MakeJByteArray(
-      env, wpi::ArrayRef(reinterpret_cast<jbyte*>(data.get()), length * 4));
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    setData
- * Signature: (I[B)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_setData
-  (JNIEnv* env, jclass, jint index, jbyteArray arr)
-{
-  JByteArrayRef jArrRef{env, arr};
-  auto arrRef = jArrRef.array();
-  HALSIM_SetAddressableLEDData(
-      index, reinterpret_cast<const HAL_AddressableLEDData*>(arrRef.data()),
-      arrRef.size() / 4);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AddressableLEDDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetAddressableLEDData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/AnalogGyroDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogGyroDataJNI.cpp
deleted file mode 100644
index 08d18de..0000000
--- a/hal/src/main/native/sim/jni/AnalogGyroDataJNI.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI.h"
-#include "mockdata/AnalogGyroData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    registerAngleCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_registerAngleCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogGyroAngleCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    cancelAngleCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_cancelAngleCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogGyroAngleCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    getAngle
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_getAngle
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogGyroAngle(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    setAngle
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_setAngle
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAnalogGyroAngle(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    registerRateCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_registerRateCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogGyroRateCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    cancelRateCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_cancelRateCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogGyroRateCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    getRate
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_getRate
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogGyroRate(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    setRate
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_setRate
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAnalogGyroRate(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogGyroInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogGyroInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogGyroInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAnalogGyroInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogGyroDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetAnalogGyroData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/AnalogInDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogInDataJNI.cpp
deleted file mode 100644
index c6cee7b..0000000
--- a/hal/src/main/native/sim/jni/AnalogInDataJNI.cpp
+++ /dev/null
@@ -1,483 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI.h"
-#include "mockdata/AnalogInData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogInInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAnalogInInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerAverageBitsCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAverageBitsCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogInAverageBitsCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelAverageBitsCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAverageBitsCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInAverageBitsCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getAverageBits
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAverageBits
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInAverageBits(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setAverageBits
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAverageBits
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetAnalogInAverageBits(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerOversampleBitsCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerOversampleBitsCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogInOversampleBitsCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelOversampleBitsCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelOversampleBitsCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInOversampleBitsCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getOversampleBits
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getOversampleBits
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInOversampleBits(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setOversampleBits
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setOversampleBits
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetAnalogInOversampleBits(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerVoltageCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerVoltageCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogInVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelVoltageCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelVoltageCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getVoltage
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getVoltage
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInVoltage(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setVoltage
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setVoltage
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAnalogInVoltage(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerAccumulatorInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogInAccumulatorInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelAccumulatorInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(
-      env, handle, index, &HALSIM_CancelAnalogInAccumulatorInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getAccumulatorInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInAccumulatorInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setAccumulatorInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAnalogInAccumulatorInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerAccumulatorValueCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorValueCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogInAccumulatorValueCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelAccumulatorValueCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorValueCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInAccumulatorValueCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getAccumulatorValue
- * Signature: (I)J
- */
-JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorValue
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInAccumulatorValue(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setAccumulatorValue
- * Signature: (IJ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorValue
-  (JNIEnv*, jclass, jint index, jlong value)
-{
-  HALSIM_SetAnalogInAccumulatorValue(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerAccumulatorCountCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorCountCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogInAccumulatorCountCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelAccumulatorCountCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorCountCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInAccumulatorCountCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getAccumulatorCount
- * Signature: (I)J
- */
-JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorCount
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInAccumulatorCount(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setAccumulatorCount
- * Signature: (IJ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorCount
-  (JNIEnv*, jclass, jint index, jlong value)
-{
-  HALSIM_SetAnalogInAccumulatorCount(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerAccumulatorCenterCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorCenterCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogInAccumulatorCenterCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelAccumulatorCenterCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorCenterCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInAccumulatorCenterCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getAccumulatorCenter
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorCenter
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInAccumulatorCenter(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setAccumulatorCenter
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorCenter
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetAnalogInAccumulatorCenter(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    registerAccumulatorDeadbandCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_registerAccumulatorDeadbandCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogInAccumulatorDeadbandCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    cancelAccumulatorDeadbandCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_cancelAccumulatorDeadbandCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogInAccumulatorDeadbandCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    getAccumulatorDeadband
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_getAccumulatorDeadband
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogInAccumulatorDeadband(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    setAccumulatorDeadband
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_setAccumulatorDeadband
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetAnalogInAccumulatorDeadband(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogInDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetAnalogInData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/AnalogOutDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogOutDataJNI.cpp
deleted file mode 100644
index af9d6d6..0000000
--- a/hal/src/main/native/sim/jni/AnalogOutDataJNI.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI.h"
-#include "mockdata/AnalogOutData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    registerVoltageCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_registerVoltageCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogOutVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    cancelVoltageCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_cancelVoltageCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogOutVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    getVoltage
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_getVoltage
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogOutVoltage(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    setVoltage
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_setVoltage
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAnalogOutVoltage(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterAnalogOutInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogOutInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogOutInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAnalogOutInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogOutDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetAnalogOutData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/AnalogTriggerDataJNI.cpp b/hal/src/main/native/sim/jni/AnalogTriggerDataJNI.cpp
deleted file mode 100644
index 66af737..0000000
--- a/hal/src/main/native/sim/jni/AnalogTriggerDataJNI.cpp
+++ /dev/null
@@ -1,181 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI.h"
-#include "mockdata/AnalogTriggerData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogTriggerInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelAnalogTriggerInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogTriggerInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetAnalogTriggerInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    registerTriggerLowerBoundCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_registerTriggerLowerBoundCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    cancelTriggerLowerBoundCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_cancelTriggerLowerBoundCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(
-      env, handle, index, &HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    getTriggerLowerBound
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_getTriggerLowerBound
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogTriggerTriggerLowerBound(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    setTriggerLowerBound
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_setTriggerLowerBound
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAnalogTriggerTriggerLowerBound(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    registerTriggerUpperBoundCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_registerTriggerUpperBoundCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    cancelTriggerUpperBoundCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_cancelTriggerUpperBoundCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(
-      env, handle, index, &HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    getTriggerUpperBound
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_getTriggerUpperBound
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetAnalogTriggerTriggerUpperBound(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    setTriggerUpperBound
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_setTriggerUpperBound
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetAnalogTriggerTriggerUpperBound(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_AnalogTriggerDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetAnalogTriggerData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/BufferCallbackStore.cpp b/hal/src/main/native/sim/jni/BufferCallbackStore.cpp
deleted file mode 100644
index 3c9941e..0000000
--- a/hal/src/main/native/sim/jni/BufferCallbackStore.cpp
+++ /dev/null
@@ -1,125 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "BufferCallbackStore.h"
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-
-using namespace wpi::java;
-using namespace sim;
-
-static hal::UnlimitedHandleResource<SIM_JniHandle, BufferCallbackStore,
-                                    hal::HAL_HandleEnum::SimulationJni>*
-    callbackHandles;
-
-namespace sim {
-void InitializeBufferStore() {
-  static hal::UnlimitedHandleResource<SIM_JniHandle, BufferCallbackStore,
-                                      hal::HAL_HandleEnum::SimulationJni>
-      cb;
-  callbackHandles = &cb;
-}
-}  // namespace sim
-
-void BufferCallbackStore::create(JNIEnv* env, jobject obj) {
-  m_call = JGlobal<jobject>(env, obj);
-}
-
-void BufferCallbackStore::performCallback(const char* name, uint8_t* buffer,
-                                          uint32_t length) {
-  JNIEnv* env;
-  JavaVM* vm = sim::GetJVM();
-  bool didAttachThread = false;
-  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
-  if (tryGetEnv == JNI_EDETACHED) {
-    // Thread not attached
-    didAttachThread = true;
-    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
-      // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
-      return;
-    }
-  } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
-  }
-
-  auto toCallbackArr =
-      MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
-                                         static_cast<size_t>(length)});
-
-  env->CallVoidMethod(m_call, sim::GetBufferCallback(), MakeJString(env, name),
-                      toCallbackArr, (jint)length);
-
-  jbyte* fromCallbackArr = reinterpret_cast<jbyte*>(
-      env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
-
-  for (size_t i = 0; i < length; i++) {
-    buffer[i] = fromCallbackArr[i];
-  }
-
-  env->ReleasePrimitiveArrayCritical(toCallbackArr, fromCallbackArr, JNI_ABORT);
-
-  if (env->ExceptionCheck()) {
-    env->ExceptionDescribe();
-  }
-
-  if (didAttachThread) {
-    vm->DetachCurrentThread();
-  }
-}
-
-void BufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
-
-SIM_JniHandle sim::AllocateBufferCallback(
-    JNIEnv* env, jint index, jobject callback,
-    RegisterBufferCallbackFunc createCallback) {
-  auto callbackStore = std::make_shared<BufferCallbackStore>();
-
-  auto handle = callbackHandles->Allocate(callbackStore);
-
-  if (handle == HAL_kInvalidHandle) {
-    return -1;
-  }
-
-  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
-  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
-
-  callbackStore->create(env, callback);
-
-  auto callbackFunc = [](const char* name, void* param, uint8_t* buffer,
-                         uint32_t length) {
-    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
-    auto data = callbackHandles->Get(handle);
-    if (!data) return;
-
-    data->performCallback(name, buffer, length);
-  };
-
-  auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
-
-  callbackStore->setCallbackId(id);
-
-  return handle;
-}
-
-void sim::FreeBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                             FreeBufferCallbackFunc freeCallback) {
-  auto callback = callbackHandles->Free(handle);
-  freeCallback(index, callback->getCallbackId());
-  callback->free(env);
-}

hal/src/main/native/sim/jni/BufferCallbackStore.h

diff --git a/hal/src/main/native/sim/jni/BufferCallbackStore.h b/hal/src/main/native/sim/jni/BufferCallbackStore.h
deleted file mode 100644
index 6b472ac..0000000
--- a/hal/src/main/native/sim/jni/BufferCallbackStore.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-
-namespace sim {
-class BufferCallbackStore {
- public:
-  void create(JNIEnv* env, jobject obj);
-  void performCallback(const char* name, uint8_t* buffer, uint32_t length);
-  void free(JNIEnv* env);
-  void setCallbackId(int32_t id) { callbackId = id; }
-  int32_t getCallbackId() { return callbackId; }
-
- private:
-  wpi::java::JGlobal<jobject> m_call;
-  int32_t callbackId;
-};
-
-void InitializeBufferStore();
-
-typedef int32_t (*RegisterBufferCallbackFunc)(int32_t index,
-                                              HAL_BufferCallback callback,
-                                              void* param);
-typedef void (*FreeBufferCallbackFunc)(int32_t index, int32_t uid);
-
-SIM_JniHandle AllocateBufferCallback(JNIEnv* env, jint index, jobject callback,
-                                     RegisterBufferCallbackFunc createCallback);
-void FreeBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                        FreeBufferCallbackFunc freeCallback);
-}  // namespace sim

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

diff --git a/hal/src/main/native/sim/jni/CallbackStore.cpp b/hal/src/main/native/sim/jni/CallbackStore.cpp
deleted file mode 100644
index 318ab1e..0000000
--- a/hal/src/main/native/sim/jni/CallbackStore.cpp
+++ /dev/null
@@ -1,194 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CallbackStore.h"
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-
-using namespace wpi::java;
-using namespace sim;
-
-static hal::UnlimitedHandleResource<SIM_JniHandle, CallbackStore,
-                                    hal::HAL_HandleEnum::SimulationJni>*
-    callbackHandles;
-
-namespace sim {
-void InitializeStore() {
-  static hal::UnlimitedHandleResource<SIM_JniHandle, CallbackStore,
-                                      hal::HAL_HandleEnum::SimulationJni>
-      cb;
-  callbackHandles = &cb;
-}
-}  // namespace sim
-
-void CallbackStore::create(JNIEnv* env, jobject obj) {
-  m_call = JGlobal<jobject>(env, obj);
-}
-
-void CallbackStore::performCallback(const char* name, const HAL_Value* value) {
-  JNIEnv* env;
-  JavaVM* vm = sim::GetJVM();
-  bool didAttachThread = false;
-  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
-  if (tryGetEnv == JNI_EDETACHED) {
-    // Thread not attached
-    didAttachThread = true;
-    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
-      // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
-      return;
-    }
-  } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
-  }
-
-  env->CallVoidMethod(m_call, sim::GetNotifyCallback(), MakeJString(env, name),
-                      (jint)value->type, (jlong)value->data.v_long,
-                      (jdouble)value->data.v_double);
-
-  if (env->ExceptionCheck()) {
-    env->ExceptionDescribe();
-  }
-
-  if (didAttachThread) {
-    vm->DetachCurrentThread();
-  }
-}
-
-void CallbackStore::free(JNIEnv* env) { m_call.free(env); }
-
-SIM_JniHandle sim::AllocateCallback(JNIEnv* env, jint index, jobject callback,
-                                    jboolean initialNotify,
-                                    RegisterCallbackFunc createCallback) {
-  auto callbackStore = std::make_shared<CallbackStore>();
-
-  auto handle = callbackHandles->Allocate(callbackStore);
-
-  if (handle == HAL_kInvalidHandle) {
-    return -1;
-  }
-
-  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
-  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
-
-  callbackStore->create(env, callback);
-
-  auto callbackFunc = [](const char* name, void* param,
-                         const HAL_Value* value) {
-    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
-    auto data = callbackHandles->Get(handle);
-    if (!data) return;
-
-    data->performCallback(name, value);
-  };
-
-  auto id = createCallback(index, callbackFunc, handleAsVoidPtr, initialNotify);
-
-  callbackStore->setCallbackId(id);
-
-  return handle;
-}
-
-void sim::FreeCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                       FreeCallbackFunc freeCallback) {
-  auto callback = callbackHandles->Free(handle);
-  freeCallback(index, callback->getCallbackId());
-  callback->free(env);
-}
-
-SIM_JniHandle sim::AllocateChannelCallback(
-    JNIEnv* env, jint index, jint channel, jobject callback,
-    jboolean initialNotify, RegisterChannelCallbackFunc createCallback) {
-  auto callbackStore = std::make_shared<CallbackStore>();
-
-  auto handle = callbackHandles->Allocate(callbackStore);
-
-  if (handle == HAL_kInvalidHandle) {
-    return -1;
-  }
-
-  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
-  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
-
-  callbackStore->create(env, callback);
-
-  auto callbackFunc = [](const char* name, void* param,
-                         const HAL_Value* value) {
-    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
-    auto data = callbackHandles->Get(handle);
-    if (!data) return;
-
-    data->performCallback(name, value);
-  };
-
-  auto id = createCallback(index, channel, callbackFunc, handleAsVoidPtr,
-                           initialNotify);
-
-  callbackStore->setCallbackId(id);
-
-  return handle;
-}
-
-void sim::FreeChannelCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                              jint channel,
-                              FreeChannelCallbackFunc freeCallback) {
-  auto callback = callbackHandles->Free(handle);
-  freeCallback(index, channel, callback->getCallbackId());
-  callback->free(env);
-}
-
-SIM_JniHandle sim::AllocateCallbackNoIndex(
-    JNIEnv* env, jobject callback, jboolean initialNotify,
-    RegisterCallbackNoIndexFunc createCallback) {
-  auto callbackStore = std::make_shared<CallbackStore>();
-
-  auto handle = callbackHandles->Allocate(callbackStore);
-
-  if (handle == HAL_kInvalidHandle) {
-    return -1;
-  }
-
-  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
-  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
-
-  callbackStore->create(env, callback);
-
-  auto callbackFunc = [](const char* name, void* param,
-                         const HAL_Value* value) {
-    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
-    auto data = callbackHandles->Get(handle);
-    if (!data) return;
-
-    data->performCallback(name, value);
-  };
-
-  auto id = createCallback(callbackFunc, handleAsVoidPtr, initialNotify);
-
-  callbackStore->setCallbackId(id);
-
-  return handle;
-}
-
-void sim::FreeCallbackNoIndex(JNIEnv* env, SIM_JniHandle handle,
-                              FreeCallbackNoIndexFunc freeCallback) {
-  auto callback = callbackHandles->Free(handle);
-  freeCallback(callback->getCallbackId());
-  callback->free(env);
-}

hal/src/main/native/sim/jni/CallbackStore.h

diff --git a/hal/src/main/native/sim/jni/CallbackStore.h b/hal/src/main/native/sim/jni/CallbackStore.h
deleted file mode 100644
index eacf314..0000000
--- a/hal/src/main/native/sim/jni/CallbackStore.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-
-namespace sim {
-class CallbackStore {
- public:
-  void create(JNIEnv* env, jobject obj);
-  void performCallback(const char* name, const HAL_Value* value);
-  void free(JNIEnv* env);
-  void setCallbackId(int32_t id) { callbackId = id; }
-  int32_t getCallbackId() { return callbackId; }
-
- private:
-  wpi::java::JGlobal<jobject> m_call;
-  int32_t callbackId;
-};
-
-void InitializeStore();
-
-typedef int32_t (*RegisterCallbackFunc)(int32_t index,
-                                        HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-typedef void (*FreeCallbackFunc)(int32_t index, int32_t uid);
-typedef int32_t (*RegisterChannelCallbackFunc)(int32_t index, int32_t channel,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-typedef void (*FreeChannelCallbackFunc)(int32_t index, int32_t channel,
-                                        int32_t uid);
-typedef int32_t (*RegisterCallbackNoIndexFunc)(HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-typedef void (*FreeCallbackNoIndexFunc)(int32_t uid);
-
-SIM_JniHandle AllocateCallback(JNIEnv* env, jint index, jobject callback,
-                               jboolean initialNotify,
-                               RegisterCallbackFunc createCallback);
-SIM_JniHandle AllocateChannelCallback(
-    JNIEnv* env, jint index, jint channel, jobject callback,
-    jboolean initialNotify, RegisterChannelCallbackFunc createCallback);
-SIM_JniHandle AllocateCallbackNoIndex(
-    JNIEnv* env, jobject callback, jboolean initialNotify,
-    RegisterCallbackNoIndexFunc createCallback);
-void FreeCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                  FreeCallbackFunc freeCallback);
-void FreeChannelCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                         jint channel, FreeChannelCallbackFunc freeCallback);
-void FreeCallbackNoIndex(JNIEnv* env, SIM_JniHandle handle,
-                         FreeCallbackNoIndexFunc freeCallback);
-}  // namespace sim

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

diff --git a/hal/src/main/native/sim/jni/ConstBufferCallbackStore.cpp b/hal/src/main/native/sim/jni/ConstBufferCallbackStore.cpp
deleted file mode 100644
index d681983..0000000
--- a/hal/src/main/native/sim/jni/ConstBufferCallbackStore.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "ConstBufferCallbackStore.h"
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-
-using namespace wpi::java;
-using namespace sim;
-
-static hal::UnlimitedHandleResource<SIM_JniHandle, ConstBufferCallbackStore,
-                                    hal::HAL_HandleEnum::SimulationJni>*
-    callbackHandles;
-
-namespace sim {
-void InitializeConstBufferStore() {
-  static hal::UnlimitedHandleResource<SIM_JniHandle, ConstBufferCallbackStore,
-                                      hal::HAL_HandleEnum::SimulationJni>
-      cb;
-  callbackHandles = &cb;
-}
-}  // namespace sim
-
-void ConstBufferCallbackStore::create(JNIEnv* env, jobject obj) {
-  m_call = JGlobal<jobject>(env, obj);
-}
-
-void ConstBufferCallbackStore::performCallback(const char* name,
-                                               const uint8_t* buffer,
-                                               uint32_t length) {
-  JNIEnv* env;
-  JavaVM* vm = sim::GetJVM();
-  bool didAttachThread = false;
-  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
-  if (tryGetEnv == JNI_EDETACHED) {
-    // Thread not attached
-    didAttachThread = true;
-    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
-      // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
-      return;
-    }
-  } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
-  }
-
-  auto toCallbackArr =
-      MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
-                                         static_cast<size_t>(length)});
-
-  env->CallVoidMethod(m_call, sim::GetConstBufferCallback(),
-                      MakeJString(env, name), toCallbackArr, (jint)length);
-
-  if (env->ExceptionCheck()) {
-    env->ExceptionDescribe();
-  }
-
-  if (didAttachThread) {
-    vm->DetachCurrentThread();
-  }
-}
-
-void ConstBufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
-
-SIM_JniHandle sim::AllocateConstBufferCallback(
-    JNIEnv* env, jint index, jobject callback,
-    RegisterConstBufferCallbackFunc createCallback) {
-  auto callbackStore = std::make_shared<ConstBufferCallbackStore>();
-
-  auto handle = callbackHandles->Allocate(callbackStore);
-
-  if (handle == HAL_kInvalidHandle) {
-    return -1;
-  }
-
-  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
-  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
-
-  callbackStore->create(env, callback);
-
-  auto callbackFunc = [](const char* name, void* param, const uint8_t* buffer,
-                         uint32_t length) {
-    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
-    auto data = callbackHandles->Get(handle);
-    if (!data) return;
-
-    data->performCallback(name, buffer, length);
-  };
-
-  auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
-
-  callbackStore->setCallbackId(id);
-
-  return handle;
-}
-
-void sim::FreeConstBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                                  FreeConstBufferCallbackFunc freeCallback) {
-  auto callback = callbackHandles->Free(handle);
-  freeCallback(index, callback->getCallbackId());
-  callback->free(env);
-}

hal/src/main/native/sim/jni/ConstBufferCallbackStore.h

diff --git a/hal/src/main/native/sim/jni/ConstBufferCallbackStore.h b/hal/src/main/native/sim/jni/ConstBufferCallbackStore.h
deleted file mode 100644
index 2164a74..0000000
--- a/hal/src/main/native/sim/jni/ConstBufferCallbackStore.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-
-namespace sim {
-class ConstBufferCallbackStore {
- public:
-  void create(JNIEnv* env, jobject obj);
-  void performCallback(const char* name, const uint8_t* buffer,
-                       uint32_t length);
-  void free(JNIEnv* env);
-  void setCallbackId(int32_t id) { callbackId = id; }
-  int32_t getCallbackId() { return callbackId; }
-
- private:
-  wpi::java::JGlobal<jobject> m_call;
-  int32_t callbackId;
-};
-
-void InitializeConstBufferStore();
-
-typedef int32_t (*RegisterConstBufferCallbackFunc)(
-    int32_t index, HAL_ConstBufferCallback callback, void* param);
-typedef void (*FreeConstBufferCallbackFunc)(int32_t index, int32_t uid);
-
-SIM_JniHandle AllocateConstBufferCallback(
-    JNIEnv* env, jint index, jobject callback,
-    RegisterConstBufferCallbackFunc createCallback);
-void FreeConstBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                             FreeConstBufferCallbackFunc freeCallback);
-}  // namespace sim

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

diff --git a/hal/src/main/native/sim/jni/DIODataJNI.cpp b/hal/src/main/native/sim/jni/DIODataJNI.cpp
deleted file mode 100644
index 2ad55f9..0000000
--- a/hal/src/main/native/sim/jni/DIODataJNI.cpp
+++ /dev/null
@@ -1,277 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_DIODataJNI.h"
-#include "mockdata/DIOData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDIOInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDIOInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDIOInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetDIOInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    registerValueCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerValueCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDIOValueCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    cancelValueCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelValueCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index, &HALSIM_CancelDIOValueCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    getValue
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getValue
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDIOValue(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    setValue
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setValue
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetDIOValue(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    registerPulseLengthCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerPulseLengthCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDIOPulseLengthCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    cancelPulseLengthCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelPulseLengthCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDIOPulseLengthCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    getPulseLength
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getPulseLength
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDIOPulseLength(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    setPulseLength
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setPulseLength
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetDIOPulseLength(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    registerIsInputCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerIsInputCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDIOIsInputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    cancelIsInputCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelIsInputCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDIOIsInputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    getIsInput
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getIsInput
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDIOIsInput(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    setIsInput
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setIsInput
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetDIOIsInput(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    registerFilterIndexCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_registerFilterIndexCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDIOFilterIndexCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    cancelFilterIndexCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_cancelFilterIndexCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDIOFilterIndexCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    getFilterIndex
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_getFilterIndex
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDIOFilterIndex(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    setFilterIndex
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_setFilterIndex
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetDIOFilterIndex(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DIODataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DIODataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetDIOData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/DigitalPWMDataJNI.cpp b/hal/src/main/native/sim/jni/DigitalPWMDataJNI.cpp
deleted file mode 100644
index 0800917..0000000
--- a/hal/src/main/native/sim/jni/DigitalPWMDataJNI.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI.h"
-#include "mockdata/DigitalPWMData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDigitalPWMInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDigitalPWMInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDigitalPWMInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetDigitalPWMInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    registerDutyCycleCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_registerDutyCycleCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDigitalPWMDutyCycleCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    cancelDutyCycleCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_cancelDutyCycleCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDigitalPWMDutyCycleCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    getDutyCycle
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_getDutyCycle
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDigitalPWMDutyCycle(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    setDutyCycle
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_setDutyCycle
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetDigitalPWMDutyCycle(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    registerPinCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_registerPinCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDigitalPWMPinCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    cancelPinCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_cancelPinCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDigitalPWMPinCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    getPin
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_getPin
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDigitalPWMPin(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    setPin
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_setPin
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetDigitalPWMPin(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DigitalPWMDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetDigitalPWMData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp b/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp
deleted file mode 100644
index 82433b3..0000000
--- a/hal/src/main/native/sim/jni/DriverStationDataJNI.cpp
+++ /dev/null
@@ -1,481 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include <cstring>
-
-#include <wpi/jni_util.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI.h"
-#include "mockdata/DriverStationData.h"
-#include "mockdata/MockHooks.h"
-
-using namespace wpi::java;
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    registerEnabledCallback
- * Signature: (Ljava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerEnabledCallback
-  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallbackNoIndex(
-      env, callback, initialNotify,
-      &HALSIM_RegisterDriverStationEnabledCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    cancelEnabledCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelEnabledCallback
-  (JNIEnv* env, jclass, jint handle)
-{
-  return sim::FreeCallbackNoIndex(env, handle,
-                                  &HALSIM_CancelDriverStationEnabledCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    getEnabled
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getEnabled
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetDriverStationEnabled();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setEnabled
- * Signature: (Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setEnabled
-  (JNIEnv*, jclass, jboolean value)
-{
-  HALSIM_SetDriverStationEnabled(value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    registerAutonomousCallback
- * Signature: (Ljava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerAutonomousCallback
-  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallbackNoIndex(
-      env, callback, initialNotify,
-      &HALSIM_RegisterDriverStationAutonomousCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    cancelAutonomousCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelAutonomousCallback
-  (JNIEnv* env, jclass, jint handle)
-{
-  return sim::FreeCallbackNoIndex(
-      env, handle, &HALSIM_CancelDriverStationAutonomousCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    getAutonomous
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getAutonomous
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetDriverStationAutonomous();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setAutonomous
- * Signature: (Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setAutonomous
-  (JNIEnv*, jclass, jboolean value)
-{
-  HALSIM_SetDriverStationAutonomous(value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    registerTestCallback
- * Signature: (Ljava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerTestCallback
-  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallbackNoIndex(
-      env, callback, initialNotify, &HALSIM_RegisterDriverStationTestCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    cancelTestCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelTestCallback
-  (JNIEnv* env, jclass, jint handle)
-{
-  return sim::FreeCallbackNoIndex(env, handle,
-                                  &HALSIM_CancelDriverStationTestCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    getTest
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getTest
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetDriverStationTest();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setTest
- * Signature: (Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setTest
-  (JNIEnv*, jclass, jboolean value)
-{
-  HALSIM_SetDriverStationTest(value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    registerEStopCallback
- * Signature: (Ljava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerEStopCallback
-  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallbackNoIndex(
-      env, callback, initialNotify, &HALSIM_RegisterDriverStationEStopCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    cancelEStopCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelEStopCallback
-  (JNIEnv* env, jclass, jint handle)
-{
-  return sim::FreeCallbackNoIndex(env, handle,
-                                  &HALSIM_CancelDriverStationEStopCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    getEStop
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getEStop
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetDriverStationEStop();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setEStop
- * Signature: (Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setEStop
-  (JNIEnv*, jclass, jboolean value)
-{
-  HALSIM_SetDriverStationEStop(value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    registerFmsAttachedCallback
- * Signature: (Ljava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerFmsAttachedCallback
-  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallbackNoIndex(
-      env, callback, initialNotify,
-      &HALSIM_RegisterDriverStationFmsAttachedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    cancelFmsAttachedCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelFmsAttachedCallback
-  (JNIEnv* env, jclass, jint handle)
-{
-  return sim::FreeCallbackNoIndex(
-      env, handle, &HALSIM_CancelDriverStationFmsAttachedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    getFmsAttached
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getFmsAttached
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetDriverStationFmsAttached();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setFmsAttached
- * Signature: (Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setFmsAttached
-  (JNIEnv*, jclass, jboolean value)
-{
-  HALSIM_SetDriverStationFmsAttached(value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    registerDsAttachedCallback
- * Signature: (Ljava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerDsAttachedCallback
-  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallbackNoIndex(
-      env, callback, initialNotify,
-      &HALSIM_RegisterDriverStationDsAttachedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    cancelDsAttachedCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_cancelDsAttachedCallback
-  (JNIEnv* env, jclass, jint handle)
-{
-  return sim::FreeCallbackNoIndex(
-      env, handle, &HALSIM_CancelDriverStationDsAttachedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    getDsAttached
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_getDsAttached
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetDriverStationDsAttached();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setDsAttached
- * Signature: (Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setDsAttached
-  (JNIEnv*, jclass, jboolean value)
-{
-  HALSIM_SetDriverStationDsAttached(value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setJoystickAxes
- * Signature: (B[F)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setJoystickAxes
-  (JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray)
-{
-  HAL_JoystickAxes axes;
-  {
-    wpi::java::JFloatArrayRef jArrayRef(env, axesArray);
-    auto arrayRef = jArrayRef.array();
-    auto arraySize = arrayRef.size();
-    int maxCount =
-        arraySize < HAL_kMaxJoystickAxes ? arraySize : HAL_kMaxJoystickAxes;
-    axes.count = maxCount;
-    for (int i = 0; i < maxCount; i++) {
-      axes.axes[i] = arrayRef[i];
-    }
-  }
-  HALSIM_SetJoystickAxes(joystickNum, &axes);
-  return;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setJoystickPOVs
- * Signature: (B[S)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setJoystickPOVs
-  (JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray)
-{
-  HAL_JoystickPOVs povs;
-  {
-    wpi::java::JShortArrayRef jArrayRef(env, povsArray);
-    auto arrayRef = jArrayRef.array();
-    auto arraySize = arrayRef.size();
-    int maxCount =
-        arraySize < HAL_kMaxJoystickPOVs ? arraySize : HAL_kMaxJoystickPOVs;
-    povs.count = maxCount;
-    for (int i = 0; i < maxCount; i++) {
-      povs.povs[i] = arrayRef[i];
-    }
-  }
-  HALSIM_SetJoystickPOVs(joystickNum, &povs);
-  return;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setJoystickButtons
- * Signature: (BII)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setJoystickButtons
-  (JNIEnv* env, jclass, jbyte joystickNum, jint buttons, jint count)
-{
-  if (count > 32) {
-    count = 32;
-  }
-  HAL_JoystickButtons joystickButtons;
-  joystickButtons.count = count;
-  joystickButtons.buttons = buttons;
-  HALSIM_SetJoystickButtons(joystickNum, &joystickButtons);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setMatchInfo
- * Signature: (Ljava/lang/String;Ljava/lang/String;III)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setMatchInfo
-  (JNIEnv* env, jclass, jstring eventName, jstring gameSpecificMessage,
-   jint matchNumber, jint replayNumber, jint matchType)
-{
-  JStringRef eventNameRef{env, eventName};
-  JStringRef gameSpecificMessageRef{env, gameSpecificMessage};
-
-  HAL_MatchInfo halMatchInfo;
-  std::snprintf(halMatchInfo.eventName, sizeof(halMatchInfo.eventName), "%s",
-                eventNameRef.c_str());
-  std::snprintf(reinterpret_cast<char*>(halMatchInfo.gameSpecificMessage),
-                sizeof(halMatchInfo.gameSpecificMessage), "%s",
-                gameSpecificMessageRef.c_str());
-  halMatchInfo.gameSpecificMessageSize = gameSpecificMessageRef.size();
-  halMatchInfo.matchType = (HAL_MatchType)matchType;
-  halMatchInfo.matchNumber = matchNumber;
-  halMatchInfo.replayNumber = replayNumber;
-  HALSIM_SetMatchInfo(&halMatchInfo);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    registerAllCallbacks
- * Signature: (Ljava/lang/Object;Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_registerAllCallbacks
-  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
-{
-  sim::AllocateCallbackNoIndex(
-      env, callback, initialNotify,
-      [](HAL_NotifyCallback cb, void* param, HAL_Bool in) {
-        HALSIM_RegisterDriverStationAllCallbacks(cb, param, in);
-        return 0;
-      });
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    notifyNewData
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_notifyNewData
-  (JNIEnv*, jclass)
-{
-  HALSIM_NotifyDriverStationNewData();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    setSendError
- * Signature: (Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_setSendError
-  (JNIEnv*, jclass, jboolean shouldSend)
-{
-  if (shouldSend) {
-    HALSIM_SetSendError(nullptr);
-  } else {
-    HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
-                           HAL_Bool isLVCode, const char* details,
-                           const char* location, const char* callStack,
-                           HAL_Bool printMsg) { return 1; });
-  }
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI
- * Method:    resetData
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DriverStationDataJNI_resetData
-  (JNIEnv*, jclass)
-{
-  HALSIM_ResetDriverStationData();
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp b/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp
deleted file mode 100644
index d746ce1..0000000
--- a/hal/src/main/native/sim/jni/DutyCycleDataJNI.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI.h"
-#include "mockdata/DutyCycleData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDutyCycleInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDutyCycleInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDutyCycleInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetDutyCycleInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    registerFrequencyCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerFrequencyCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDutyCycleFrequencyCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    cancelFrequencyCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelFrequencyCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDutyCycleFrequencyCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    getFrequency
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getFrequency
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDutyCycleFrequency(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    setFrequency
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setFrequency
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetDutyCycleFrequency(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    registerOutputCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_registerOutputCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterDutyCycleOutputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    cancelOutputCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_cancelOutputCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelDutyCycleOutputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    getOutput
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_getOutput
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetDutyCycleOutput(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    setOutput
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_setOutput
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetDutyCycleOutput(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_DutyCycleDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetDutyCycleData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/EncoderDataJNI.cpp b/hal/src/main/native/sim/jni/EncoderDataJNI.cpp
deleted file mode 100644
index 8992b0c..0000000
--- a/hal/src/main/native/sim/jni/EncoderDataJNI.cpp
+++ /dev/null
@@ -1,428 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_EncoderDataJNI.h"
-#include "mockdata/EncoderData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetEncoderInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerCountCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerCountCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderCountCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelCountCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelCountCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderCountCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getCount
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getCount
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderCount(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setCount
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setCount
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetEncoderCount(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerPeriodCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerPeriodCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderPeriodCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelPeriodCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelPeriodCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderPeriodCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getPeriod
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getPeriod
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderPeriod(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setPeriod
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setPeriod
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetEncoderPeriod(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerResetCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerResetCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderResetCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelResetCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelResetCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderResetCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getReset
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getReset
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderReset(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setReset
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setReset
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetEncoderReset(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerMaxPeriodCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerMaxPeriodCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderMaxPeriodCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelMaxPeriodCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelMaxPeriodCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderMaxPeriodCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getMaxPeriod
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getMaxPeriod
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderMaxPeriod(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setMaxPeriod
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setMaxPeriod
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetEncoderMaxPeriod(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerDirectionCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerDirectionCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderDirectionCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelDirectionCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelDirectionCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderDirectionCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getDirection
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getDirection
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderDirection(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setDirection
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setDirection
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetEncoderDirection(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerReverseDirectionCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerReverseDirectionCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderReverseDirectionCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelReverseDirectionCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelReverseDirectionCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderReverseDirectionCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getReverseDirection
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getReverseDirection
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderReverseDirection(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setReverseDirection
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setReverseDirection
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetEncoderReverseDirection(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    registerSamplesToAverageCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_registerSamplesToAverageCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterEncoderSamplesToAverageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    cancelSamplesToAverageCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_cancelSamplesToAverageCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelEncoderSamplesToAverageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    getSamplesToAverage
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_getSamplesToAverage
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetEncoderSamplesToAverage(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    setSamplesToAverage
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_setSamplesToAverage
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetEncoderSamplesToAverage(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_EncoderDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_EncoderDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetEncoderData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/I2CDataJNI.cpp b/hal/src/main/native/sim/jni/I2CDataJNI.cpp
deleted file mode 100644
index 14b3292..0000000
--- a/hal/src/main/native/sim/jni/I2CDataJNI.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "BufferCallbackStore.h"
-#include "CallbackStore.h"
-#include "ConstBufferCallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_I2CDataJNI.h"
-#include "mockdata/I2CData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterI2CInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelI2CInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetI2CInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetI2CInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    registerReadCallback
- * Signature: (ILjava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_registerReadCallback
-  (JNIEnv* env, jclass, jint index, jobject callback)
-{
-  return sim::AllocateBufferCallback(env, index, callback,
-                                     &HALSIM_RegisterI2CReadCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    cancelReadCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_cancelReadCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  sim::FreeBufferCallback(env, handle, index, &HALSIM_CancelI2CReadCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    registerWriteCallback
- * Signature: (ILjava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_registerWriteCallback
-  (JNIEnv* env, jclass, jint index, jobject callback)
-{
-  return sim::AllocateConstBufferCallback(env, index, callback,
-                                          &HALSIM_RegisterI2CWriteCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    cancelWriteCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_cancelWriteCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  sim::FreeConstBufferCallback(env, handle, index,
-                               &HALSIM_CancelI2CWriteCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_I2CDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_I2CDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetI2CData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/NotifierDataJNI.cpp b/hal/src/main/native/sim/jni/NotifierDataJNI.cpp
deleted file mode 100644
index b59b6e0..0000000
--- a/hal/src/main/native/sim/jni/NotifierDataJNI.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "edu_wpi_first_hal_sim_mockdata_NotifierDataJNI.h"
-#include "mockdata/NotifierData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_NotifierDataJNI
- * Method:    getNextTimeout
- * Signature: ()J
- */
-JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_NotifierDataJNI_getNextTimeout
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetNextNotifierTimeout();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_NotifierDataJNI
- * Method:    getNumNotifiers
- * Signature: ()I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_NotifierDataJNI_getNumNotifiers
-  (JNIEnv*, jclass)
-{
-  return HALSIM_GetNumNotifiers();
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/PCMDataJNI.cpp b/hal/src/main/native/sim/jni/PCMDataJNI.cpp
deleted file mode 100644
index de6f738..0000000
--- a/hal/src/main/native/sim/jni/PCMDataJNI.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_PCMDataJNI.h"
-#include "mockdata/PCMData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerSolenoidInitializedCallback
- * Signature: (IILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerSolenoidInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  return sim::AllocateChannelCallback(
-      env, index, channel, callback, initialNotify,
-      &HALSIM_RegisterPCMSolenoidInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    cancelSolenoidInitializedCallback
- * Signature: (III)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelSolenoidInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
-{
-  return sim::FreeChannelCallback(env, handle, index, channel,
-                                  &HALSIM_CancelPCMSolenoidInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    getSolenoidInitialized
- * Signature: (II)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getSolenoidInitialized
-  (JNIEnv*, jclass, jint index, jint channel)
-{
-  return HALSIM_GetPCMSolenoidInitialized(index, channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    setSolenoidInitialized
- * Signature: (IIZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setSolenoidInitialized
-  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
-{
-  HALSIM_SetPCMSolenoidInitialized(index, channel, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerSolenoidOutputCallback
- * Signature: (IILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerSolenoidOutputCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  return sim::AllocateChannelCallback(
-      env, index, channel, callback, initialNotify,
-      &HALSIM_RegisterPCMSolenoidOutputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    cancelSolenoidOutputCallback
- * Signature: (III)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelSolenoidOutputCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
-{
-  return sim::FreeChannelCallback(env, handle, index, channel,
-                                  &HALSIM_CancelPCMSolenoidOutputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    getSolenoidOutput
- * Signature: (II)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getSolenoidOutput
-  (JNIEnv*, jclass, jint index, jint channel)
-{
-  return HALSIM_GetPCMSolenoidOutput(index, channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    setSolenoidOutput
- * Signature: (IIZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setSolenoidOutput
-  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
-{
-  HALSIM_SetPCMSolenoidOutput(index, channel, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerCompressorInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerCompressorInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterPCMCompressorInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    cancelCompressorInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelCompressorInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMCompressorInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    getCompressorInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getCompressorInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMCompressorInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    setCompressorInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setCompressorInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMCompressorInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerCompressorOnCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerCompressorOnCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMCompressorOnCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    cancelCompressorOnCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelCompressorOnCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMCompressorOnCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    getCompressorOn
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getCompressorOn
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMCompressorOn(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    setCompressorOn
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setCompressorOn
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMCompressorOn(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerClosedLoopEnabledCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerClosedLoopEnabledCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMClosedLoopEnabledCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    cancelClosedLoopEnabledCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelClosedLoopEnabledCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMClosedLoopEnabledCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    getClosedLoopEnabled
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getClosedLoopEnabled
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMClosedLoopEnabled(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    setClosedLoopEnabled
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setClosedLoopEnabled
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMClosedLoopEnabled(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerPressureSwitchCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerPressureSwitchCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMPressureSwitchCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    cancelPressureSwitchCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelPressureSwitchCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMPressureSwitchCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    getPressureSwitch
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getPressureSwitch
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMPressureSwitch(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    setPressureSwitch
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setPressureSwitch
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMPressureSwitch(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerCompressorCurrentCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerCompressorCurrentCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMCompressorCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    cancelCompressorCurrentCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_cancelCompressorCurrentCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMCompressorCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    getCompressorCurrent
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_getCompressorCurrent
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMCompressorCurrent(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    setCompressorCurrent
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_setCompressorCurrent
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPCMCompressorCurrent(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerAllNonSolenoidCallbacks
- * Signature: (ILjava/lang/Object;Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerAllNonSolenoidCallbacks
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      [](int32_t index, HAL_NotifyCallback cb, void* param, HAL_Bool in) {
-        HALSIM_RegisterPCMAllNonSolenoidCallbacks(index, cb, param, in);
-        return 0;
-      });
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    registerAllSolenoidCallbacks
- * Signature: (IILjava/lang/Object;Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_registerAllSolenoidCallbacks
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  sim::AllocateChannelCallback(
-      env, index, channel, callback, initialNotify,
-      [](int32_t index, int32_t channel, HAL_NotifyCallback cb, void* param,
-         HAL_Bool in) {
-        HALSIM_RegisterPCMAllSolenoidCallbacks(index, channel, cb, param, in);
-        return 0;
-      });
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PCMDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PCMDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetPCMData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/PDPDataJNI.cpp b/hal/src/main/native/sim/jni/PDPDataJNI.cpp
deleted file mode 100644
index 5d39e87..0000000
--- a/hal/src/main/native/sim/jni/PDPDataJNI.cpp
+++ /dev/null
@@ -1,230 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_PDPDataJNI.h"
-#include "mockdata/PDPData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPDPInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPDPInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPDPInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPDPInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    registerTemperatureCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerTemperatureCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPDPTemperatureCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    cancelTemperatureCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelTemperatureCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPDPTemperatureCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    getTemperature
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getTemperature
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPDPTemperature(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    setTemperature
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setTemperature
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPDPTemperature(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    registerVoltageCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerVoltageCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPDPVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    cancelVoltageCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelVoltageCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPDPVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    getVoltage
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getVoltage
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPDPVoltage(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    setVoltage
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setVoltage
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPDPVoltage(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    registerCurrentCallback
- * Signature: (IILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_registerCurrentCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  return sim::AllocateChannelCallback(env, index, channel, callback,
-                                      initialNotify,
-                                      &HALSIM_RegisterPDPCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    cancelCurrentCallback
- * Signature: (III)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_cancelCurrentCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
-{
-  return sim::FreeChannelCallback(env, handle, index, channel,
-                                  &HALSIM_CancelPDPCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    getCurrent
- * Signature: (II)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_getCurrent
-  (JNIEnv*, jclass, jint index, jint channel)
-{
-  return HALSIM_GetPDPCurrent(index, channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    setCurrent
- * Signature: (IID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_setCurrent
-  (JNIEnv*, jclass, jint index, jint channel, jdouble value)
-{
-  HALSIM_SetPDPCurrent(index, channel, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PDPDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PDPDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetPDPData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/PWMDataJNI.cpp b/hal/src/main/native/sim/jni/PWMDataJNI.cpp
deleted file mode 100644
index b8a7c41..0000000
--- a/hal/src/main/native/sim/jni/PWMDataJNI.cpp
+++ /dev/null
@@ -1,327 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_PWMDataJNI.h"
-#include "mockdata/PWMData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPWMInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPWMInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPWMInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPWMInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    registerRawValueCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerRawValueCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPWMRawValueCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    cancelRawValueCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelRawValueCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPWMRawValueCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    getRawValue
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getRawValue
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPWMRawValue(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    setRawValue
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setRawValue
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetPWMRawValue(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    registerSpeedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerSpeedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPWMSpeedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    cancelSpeedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelSpeedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index, &HALSIM_CancelPWMSpeedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    getSpeed
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getSpeed
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPWMSpeed(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    setSpeed
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setSpeed
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPWMSpeed(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    registerPositionCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerPositionCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPWMPositionCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    cancelPositionCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelPositionCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPWMPositionCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    getPosition
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getPosition
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPWMPosition(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    setPosition
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setPosition
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPWMPosition(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    registerPeriodScaleCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerPeriodScaleCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPWMPeriodScaleCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    cancelPeriodScaleCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelPeriodScaleCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPWMPeriodScaleCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    getPeriodScale
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getPeriodScale
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPWMPeriodScale(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    setPeriodScale
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setPeriodScale
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetPWMPeriodScale(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    registerZeroLatchCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_registerZeroLatchCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPWMZeroLatchCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    cancelZeroLatchCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_cancelZeroLatchCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPWMZeroLatchCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    getZeroLatch
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_getZeroLatch
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPWMZeroLatch(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    setZeroLatch
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_setZeroLatch
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPWMZeroLatch(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_PWMDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_PWMDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetPWMData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/RelayDataJNI.cpp b/hal/src/main/native/sim/jni/RelayDataJNI.cpp
deleted file mode 100644
index bf85407..0000000
--- a/hal/src/main/native/sim/jni/RelayDataJNI.cpp
+++ /dev/null
@@ -1,228 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_RelayDataJNI.h"
-#include "mockdata/RelayData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    registerInitializedForwardCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerInitializedForwardCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRelayInitializedForwardCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    cancelInitializedForwardCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelInitializedForwardCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRelayInitializedForwardCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    getInitializedForward
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getInitializedForward
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRelayInitializedForward(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    setInitializedForward
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setInitializedForward
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRelayInitializedForward(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    registerInitializedReverseCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerInitializedReverseCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRelayInitializedReverseCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    cancelInitializedReverseCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelInitializedReverseCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRelayInitializedReverseCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    getInitializedReverse
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getInitializedReverse
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRelayInitializedReverse(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    setInitializedReverse
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setInitializedReverse
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRelayInitializedReverse(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    registerForwardCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerForwardCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRelayForwardCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    cancelForwardCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelForwardCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRelayForwardCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    getForward
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getForward
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRelayForward(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    setForward
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setForward
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRelayForward(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    registerReverseCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_registerReverseCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRelayReverseCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    cancelReverseCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_cancelReverseCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRelayReverseCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    getReverse
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_getReverse
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRelayReverse(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    setReverse
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_setReverse
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRelayReverse(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RelayDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RelayDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetRelayData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/RoboRioDataJNI.cpp b/hal/src/main/native/sim/jni/RoboRioDataJNI.cpp
deleted file mode 100644
index d69e895..0000000
--- a/hal/src/main/native/sim/jni/RoboRioDataJNI.cpp
+++ /dev/null
@@ -1,778 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI.h"
-#include "mockdata/RoboRioData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerFPGAButtonCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerFPGAButtonCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioFPGAButtonCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelFPGAButtonCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelFPGAButtonCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioFPGAButtonCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getFPGAButton
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getFPGAButton
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioFPGAButton(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setFPGAButton
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setFPGAButton
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRoboRioFPGAButton(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerVInVoltageCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerVInVoltageCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioVInVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelVInVoltageCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelVInVoltageCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioVInVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getVInVoltage
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getVInVoltage
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioVInVoltage(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setVInVoltage
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setVInVoltage
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioVInVoltage(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerVInCurrentCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerVInCurrentCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioVInCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelVInCurrentCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelVInCurrentCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioVInCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getVInCurrent
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getVInCurrent
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioVInCurrent(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setVInCurrent
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setVInCurrent
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioVInCurrent(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserVoltage6VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserVoltage6VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserVoltage6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserVoltage6VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserVoltage6VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserVoltage6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserVoltage6V
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserVoltage6V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserVoltage6V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserVoltage6V
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserVoltage6V
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioUserVoltage6V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserCurrent6VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserCurrent6VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserCurrent6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserCurrent6VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserCurrent6VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserCurrent6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserCurrent6V
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserCurrent6V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserCurrent6V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserCurrent6V
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserCurrent6V
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioUserCurrent6V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserActive6VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserActive6VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserActive6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserActive6VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserActive6VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserActive6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserActive6V
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserActive6V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserActive6V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserActive6V
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserActive6V
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRoboRioUserActive6V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserVoltage5VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserVoltage5VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserVoltage5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserVoltage5VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserVoltage5VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserVoltage5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserVoltage5V
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserVoltage5V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserVoltage5V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserVoltage5V
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserVoltage5V
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioUserVoltage5V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserCurrent5VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserCurrent5VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserCurrent5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserCurrent5VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserCurrent5VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserCurrent5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserCurrent5V
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserCurrent5V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserCurrent5V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserCurrent5V
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserCurrent5V
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioUserCurrent5V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserActive5VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserActive5VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserActive5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserActive5VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserActive5VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserActive5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserActive5V
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserActive5V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserActive5V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserActive5V
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserActive5V
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRoboRioUserActive5V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserVoltage3V3Callback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserVoltage3V3Callback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserVoltage3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserVoltage3V3Callback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserVoltage3V3Callback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserVoltage3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserVoltage3V3
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserVoltage3V3
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserVoltage3V3(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserVoltage3V3
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserVoltage3V3
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioUserVoltage3V3(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserCurrent3V3Callback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserCurrent3V3Callback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserCurrent3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserCurrent3V3Callback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserCurrent3V3Callback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserCurrent3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserCurrent3V3
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserCurrent3V3
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserCurrent3V3(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserCurrent3V3
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserCurrent3V3
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetRoboRioUserCurrent3V3(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserActive3V3Callback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserActive3V3Callback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserActive3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserActive3V3Callback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserActive3V3Callback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserActive3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserActive3V3
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserActive3V3
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserActive3V3(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserActive3V3
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserActive3V3
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetRoboRioUserActive3V3(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserFaults6VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserFaults6VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserFaults6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserFaults6VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserFaults6VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserFaults6VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserFaults6V
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserFaults6V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserFaults6V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserFaults6V
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserFaults6V
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetRoboRioUserFaults6V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserFaults5VCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserFaults5VCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserFaults5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserFaults5VCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserFaults5VCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserFaults5VCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserFaults5V
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserFaults5V
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserFaults5V(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserFaults5V
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserFaults5V
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetRoboRioUserFaults5V(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    registerUserFaults3V3Callback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_registerUserFaults3V3Callback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterRoboRioUserFaults3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    cancelUserFaults3V3Callback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_cancelUserFaults3V3Callback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelRoboRioUserFaults3V3Callback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    getUserFaults3V3
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_getUserFaults3V3
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetRoboRioUserFaults3V3(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    setUserFaults3V3
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_setUserFaults3V3
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetRoboRioUserFaults3V3(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_RoboRioDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetRoboRioData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/SPIAccelerometerDataJNI.cpp b/hal/src/main/native/sim/jni/SPIAccelerometerDataJNI.cpp
deleted file mode 100644
index ca12f79..0000000
--- a/hal/src/main/native/sim/jni/SPIAccelerometerDataJNI.cpp
+++ /dev/null
@@ -1,278 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI.h"
-#include "mockdata/SPIAccelerometerData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    registerActiveCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerActiveCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterSPIAccelerometerActiveCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    cancelActiveCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelActiveCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelSPIAccelerometerActiveCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    getActive
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getActive
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetSPIAccelerometerActive(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    setActive
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setActive
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetSPIAccelerometerActive(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    registerRangeCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerRangeCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterSPIAccelerometerRangeCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    cancelRangeCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelRangeCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelSPIAccelerometerRangeCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    getRange
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getRange
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetSPIAccelerometerRange(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    setRange
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setRange
-  (JNIEnv*, jclass, jint index, jint value)
-{
-  HALSIM_SetSPIAccelerometerRange(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    registerXCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerXCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterSPIAccelerometerXCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    cancelXCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelXCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelSPIAccelerometerXCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    getX
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getX
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetSPIAccelerometerX(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    setX
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setX
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetSPIAccelerometerX(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    registerYCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerYCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterSPIAccelerometerYCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    cancelYCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelYCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelSPIAccelerometerYCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    getY
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getY
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetSPIAccelerometerY(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    setY
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setY
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetSPIAccelerometerY(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    registerZCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_registerZCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterSPIAccelerometerZCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    cancelZCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_cancelZCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelSPIAccelerometerZCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    getZ
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_getZ
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetSPIAccelerometerZ(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    setZ
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_setZ
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetSPIAccelerometerZ(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIAccelerometerDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetSPIAccelerometerData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/SPIDataJNI.cpp b/hal/src/main/native/sim/jni/SPIDataJNI.cpp
deleted file mode 100644
index 4eb342c..0000000
--- a/hal/src/main/native/sim/jni/SPIDataJNI.cpp
+++ /dev/null
@@ -1,158 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "BufferCallbackStore.h"
-#include "CallbackStore.h"
-#include "ConstBufferCallbackStore.h"
-#include "SpiReadAutoReceiveBufferCallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_SPIDataJNI.h"
-#include "mockdata/SPIData.h"
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterSPIInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelSPIInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetSPIInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetSPIInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    registerReadCallback
- * Signature: (ILjava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerReadCallback
-  (JNIEnv* env, jclass, jint index, jobject callback)
-{
-  return sim::AllocateBufferCallback(env, index, callback,
-                                     &HALSIM_RegisterSPIReadCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    cancelReadCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelReadCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  sim::FreeBufferCallback(env, handle, index, &HALSIM_CancelSPIReadCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    registerWriteCallback
- * Signature: (ILjava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerWriteCallback
-  (JNIEnv* env, jclass, jint index, jobject callback)
-{
-  return sim::AllocateConstBufferCallback(env, index, callback,
-                                          &HALSIM_RegisterSPIWriteCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    cancelWriteCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelWriteCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  sim::FreeConstBufferCallback(env, handle, index,
-                               &HALSIM_CancelSPIWriteCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    registerReadAutoReceiveBufferCallback
- * Signature: (ILjava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_registerReadAutoReceiveBufferCallback
-  (JNIEnv* env, jclass, jint index, jobject callback)
-{
-  return sim::AllocateSpiBufferCallback(
-      env, index, callback, &HALSIM_RegisterSPIReadAutoReceivedDataCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    cancelReadAutoReceiveBufferCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_cancelReadAutoReceiveBufferCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  sim::FreeSpiBufferCallback(env, handle, index,
-                             &HALSIM_CancelSPIReadAutoReceivedDataCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SPIDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SPIDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetSPIData(index);
-}
-
-}  // extern "C"

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

diff --git a/hal/src/main/native/sim/jni/SimDeviceDataJNI.cpp b/hal/src/main/native/sim/jni/SimDeviceDataJNI.cpp
deleted file mode 100644
index f6cd05e..0000000
--- a/hal/src/main/native/sim/jni/SimDeviceDataJNI.cpp
+++ /dev/null
@@ -1,573 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "SimDeviceDataJNI.h"
-
-#include <jni.h>
-
-#include <functional>
-#include <string>
-#include <utility>
-
-#include <wpi/UidVector.h>
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI.h"
-#include "mockdata/SimDeviceData.h"
-
-using namespace wpi::java;
-
-static JClass simDeviceInfoCls;
-static JClass simValueInfoCls;
-static JClass simDeviceCallbackCls;
-static JClass simValueCallbackCls;
-static jmethodID simDeviceCallbackCallback;
-static jmethodID simValueCallbackCallback;
-
-namespace {
-
-struct DeviceInfo {
-  DeviceInfo(const char* name_, HAL_SimDeviceHandle handle_)
-      : name{name_}, handle{handle_} {}
-  std::string name;
-  HAL_SimValueHandle handle;
-
-  jobject MakeJava(JNIEnv* env) const;
-  void CallJava(JNIEnv* env, jobject callobj) const;
-};
-
-struct ValueInfo {
-  ValueInfo(const char* name_, HAL_SimValueHandle handle_, bool readonly_,
-            const HAL_Value& value_)
-      : name{name_}, handle{handle_}, readonly{readonly_}, value{value_} {}
-  std::string name;
-  HAL_SimValueHandle handle;
-  bool readonly;
-  HAL_Value value;
-
-  jobject MakeJava(JNIEnv* env) const;
-  void CallJava(JNIEnv* env, jobject callobj) const;
-
- private:
-  std::pair<jlong, jdouble> ToValue12() const;
-};
-
-}  // namespace
-
-jobject DeviceInfo::MakeJava(JNIEnv* env) const {
-  static jmethodID func =
-      env->GetMethodID(simDeviceInfoCls, "<init>", "(Ljava/lang/String;I)V");
-  return env->NewObject(simDeviceInfoCls, func, MakeJString(env, name),
-                        (jint)handle);
-}
-
-void DeviceInfo::CallJava(JNIEnv* env, jobject callobj) const {
-  env->CallVoidMethod(callobj, simDeviceCallbackCallback,
-                      MakeJString(env, name), (jint)handle);
-}
-
-std::pair<jlong, jdouble> ValueInfo::ToValue12() const {
-  jlong value1 = 0;
-  jdouble value2 = 0.0;
-  switch (value.type) {
-    case HAL_BOOLEAN:
-      value1 = value.data.v_boolean;
-      break;
-    case HAL_DOUBLE:
-      value2 = value.data.v_double;
-      break;
-    case HAL_ENUM:
-      value1 = value.data.v_enum;
-      break;
-    case HAL_INT:
-      value1 = value.data.v_int;
-      break;
-    case HAL_LONG:
-      value1 = value.data.v_long;
-      break;
-    default:
-      break;
-  }
-  return std::pair(value1, value2);
-}
-
-jobject ValueInfo::MakeJava(JNIEnv* env) const {
-  static jmethodID func =
-      env->GetMethodID(simValueInfoCls, "<init>", "(Ljava/lang/String;IZIJD)V");
-  auto [value1, value2] = ToValue12();
-  return env->NewObject(simValueInfoCls, func, MakeJString(env, name),
-                        (jint)handle, (jboolean)readonly, (jint)value.type,
-                        value1, value2);
-}
-
-void ValueInfo::CallJava(JNIEnv* env, jobject callobj) const {
-  auto [value1, value2] = ToValue12();
-  env->CallVoidMethod(callobj, simValueCallbackCallback, MakeJString(env, name),
-                      (jint)handle, (jboolean)readonly, (jint)value.type,
-                      value1, value2);
-}
-
-namespace {
-
-class CallbackStore {
- public:
-  explicit CallbackStore(JNIEnv* env, jobject obj) : m_call{env, obj} {}
-  ~CallbackStore() {
-    if (m_cancelCallback) m_cancelCallback();
-  }
-
-  void SetCancel(std::function<void()> cancelCallback) {
-    m_cancelCallback = std::move(cancelCallback);
-  }
-  void Free(JNIEnv* env) { m_call.free(env); }
-  jobject Get() const { return m_call; }
-
- private:
-  wpi::java::JGlobal<jobject> m_call;
-  std::function<void()> m_cancelCallback;
-};
-
-class CallbackThreadJNI : public wpi::SafeThread {
- public:
-  void Main();
-
-  using DeviceCalls =
-      std::vector<std::pair<std::weak_ptr<CallbackStore>, DeviceInfo>>;
-  DeviceCalls m_deviceCalls;
-  using ValueCalls =
-      std::vector<std::pair<std::weak_ptr<CallbackStore>, ValueInfo>>;
-  ValueCalls m_valueCalls;
-
-  wpi::UidVector<std::shared_ptr<CallbackStore>, 4> m_callbacks;
-};
-
-class CallbackJNI {
- public:
-  static CallbackJNI& GetInstance() {
-    static CallbackJNI inst;
-    return inst;
-  }
-  void SendDevice(int32_t callback, DeviceInfo info);
-  void SendValue(int32_t callback, ValueInfo info);
-
-  std::pair<int32_t, std::shared_ptr<CallbackStore>> AllocateCallback(
-      JNIEnv* env, jobject obj);
-
-  void FreeCallback(JNIEnv* env, int32_t uid);
-
- private:
-  CallbackJNI() { m_owner.Start(); }
-
-  wpi::SafeThreadOwner<CallbackThreadJNI> m_owner;
-};
-
-}  // namespace
-
-void CallbackThreadJNI::Main() {
-  JNIEnv* env;
-  JavaVMAttachArgs args;
-  args.version = JNI_VERSION_1_2;
-  args.name = const_cast<char*>("SimDeviceCallback");
-  args.group = nullptr;
-  jint rs = sim::GetJVM()->AttachCurrentThreadAsDaemon(
-      reinterpret_cast<void**>(&env), &args);
-  if (rs != JNI_OK) return;
-
-  DeviceCalls deviceCalls;
-  ValueCalls valueCalls;
-
-  std::unique_lock lock(m_mutex);
-  while (m_active) {
-    m_cond.wait(lock, [&] { return !m_active; });
-    if (!m_active) break;
-
-    deviceCalls.swap(m_deviceCalls);
-    valueCalls.swap(m_valueCalls);
-
-    lock.unlock();  // don't hold mutex during callback execution
-
-    for (auto&& call : deviceCalls) {
-      if (auto store = call.first.lock()) {
-        if (jobject callobj = store->Get()) {
-          call.second.CallJava(env, callobj);
-          if (env->ExceptionCheck()) {
-            env->ExceptionDescribe();
-            env->ExceptionClear();
-          }
-        }
-      }
-    }
-
-    for (auto&& call : valueCalls) {
-      if (auto store = call.first.lock()) {
-        if (jobject callobj = store->Get()) {
-          call.second.CallJava(env, callobj);
-          if (env->ExceptionCheck()) {
-            env->ExceptionDescribe();
-            env->ExceptionClear();
-          }
-        }
-      }
-    }
-
-    deviceCalls.clear();
-    valueCalls.clear();
-
-    lock.lock();
-  }
-
-  // free global references
-  for (auto&& callback : m_callbacks) callback->Free(env);
-
-  sim::GetJVM()->DetachCurrentThread();
-}
-
-void CallbackJNI::SendDevice(int32_t callback, DeviceInfo info) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_deviceCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
-  thr->m_cond.notify_one();
-}
-
-void CallbackJNI::SendValue(int32_t callback, ValueInfo info) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_valueCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
-  thr->m_cond.notify_one();
-}
-
-std::pair<int32_t, std::shared_ptr<CallbackStore>>
-CallbackJNI::AllocateCallback(JNIEnv* env, jobject obj) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return std::pair(0, nullptr);
-  auto store = std::make_shared<CallbackStore>(env, obj);
-  return std::pair(thr->m_callbacks.emplace_back(store) + 1, store);
-}
-
-void CallbackJNI::FreeCallback(JNIEnv* env, int32_t uid) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  if (uid <= 0 || static_cast<uint32_t>(uid) >= thr->m_callbacks.size()) return;
-  --uid;
-  auto store = std::move(thr->m_callbacks[uid]);
-  thr->m_callbacks.erase(uid);
-  store->Free(env);
-}
-
-namespace sim {
-
-bool InitializeSimDeviceDataJNI(JNIEnv* env) {
-  simDeviceInfoCls = JClass(
-      env, "edu/wpi/first/hal/sim/mockdata/SimDeviceDataJNI$SimDeviceInfo");
-  if (!simDeviceInfoCls) return false;
-
-  simValueInfoCls = JClass(
-      env, "edu/wpi/first/hal/sim/mockdata/SimDeviceDataJNI$SimValueInfo");
-  if (!simValueInfoCls) return false;
-
-  simDeviceCallbackCls = JClass(env, "edu/wpi/first/hal/sim/SimDeviceCallback");
-  if (!simDeviceCallbackCls) return false;
-
-  simDeviceCallbackCallback = env->GetMethodID(simDeviceCallbackCls, "callback",
-                                               "(Ljava/lang/String;I)V");
-  if (!simDeviceCallbackCallback) return false;
-
-  simValueCallbackCls = JClass(env, "edu/wpi/first/hal/sim/SimValueCallback");
-  if (!simValueCallbackCls) return false;
-
-  simValueCallbackCallback = env->GetMethodID(
-      simValueCallbackCls, "callbackNative", "(Ljava/lang/String;IZIJD)V");
-  if (!simValueCallbackCallback) return false;
-
-  return true;
-}
-
-void FreeSimDeviceDataJNI(JNIEnv* env) {
-  simDeviceInfoCls.free(env);
-  simValueInfoCls.free(env);
-  simDeviceCallbackCls.free(env);
-  simValueCallbackCls.free(env);
-}
-
-}  // namespace sim
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    registerSimDeviceCreatedCallback
- * Signature: (Ljava/lang/String;Ljava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimDeviceCreatedCallback
-  (JNIEnv* env, jclass, jstring prefix, jobject callback,
-   jboolean initialNotify)
-{
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimDeviceCreatedCallback(
-      JStringRef{env, prefix}.c_str(),
-      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimDeviceCreatedCallback(cuid); });
-  return uid;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    cancelSimDeviceCreatedCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimDeviceCreatedCallback
-  (JNIEnv* env, jclass, jint uid)
-{
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    registerSimDeviceFreedCallback
- * Signature: (Ljava/lang/String;Ljava/lang/Object;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimDeviceFreedCallback
-  (JNIEnv* env, jclass, jstring prefix, jobject callback)
-{
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimDeviceFreedCallback(
-      JStringRef{env, prefix}.c_str(),
-      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
-      });
-  store->SetCancel([cuid] { HALSIM_CancelSimDeviceFreedCallback(cuid); });
-  return uid;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    cancelSimDeviceFreedCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimDeviceFreedCallback
-  (JNIEnv* env, jclass, jint uid)
-{
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    getSimDeviceHandle
- * Signature: (Ljava/lang/String;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimDeviceHandle
-  (JNIEnv* env, jclass, jstring name)
-{
-  return HALSIM_GetSimDeviceHandle(JStringRef{env, name}.c_str());
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    getSimValueDeviceHandle
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimValueDeviceHandle
-  (JNIEnv*, jclass, jint handle)
-{
-  return HALSIM_GetSimValueDeviceHandle(handle);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    enumerateSimDevices
- * Signature: (Ljava/lang/String;)[Ljava/lang/Object;
- */
-JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_enumerateSimDevices
-  (JNIEnv* env, jclass, jstring prefix)
-{
-  // get values
-  std::vector<DeviceInfo> arr;
-  HALSIM_EnumerateSimDevices(
-      JStringRef{env, prefix}.c_str(), &arr,
-      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-        auto arr = static_cast<std::vector<DeviceInfo>*>(param);
-        arr->emplace_back(name, handle);
-      });
-
-  // convert to java
-  size_t numElems = arr.size();
-  jobjectArray jarr =
-      env->NewObjectArray(arr.size(), simDeviceInfoCls, nullptr);
-  if (!jarr) return nullptr;
-  for (size_t i = 0; i < numElems; ++i) {
-    JLocal<jobject> elem{env, arr[i].MakeJava(env)};
-    env->SetObjectArrayElement(jarr, i, elem.obj());
-  }
-  return jarr;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    registerSimValueCreatedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimValueCreatedCallback
-  (JNIEnv* env, jclass, jint device, jobject callback, jboolean initialNotify)
-{
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimValueCreatedCallback(
-      device, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimValueHandle handle,
-         HAL_Bool readonly, const HAL_Value* value) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendValue(
-            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimValueCreatedCallback(cuid); });
-  return uid;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    cancelSimValueCreatedCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimValueCreatedCallback
-  (JNIEnv* env, jclass, jint uid)
-{
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    registerSimValueChangedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_registerSimValueChangedCallback
-  (JNIEnv* env, jclass, jint handle, jobject callback, jboolean initialNotify)
-{
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimValueChangedCallback(
-      handle, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimValueHandle handle,
-         HAL_Bool readonly, const HAL_Value* value) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendValue(
-            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimValueChangedCallback(cuid); });
-  return uid;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    cancelSimValueChangedCallback
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_cancelSimValueChangedCallback
-  (JNIEnv* env, jclass, jint uid)
-{
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    getSimValueHandle
- * Signature: (ILjava/lang/String;)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimValueHandle
-  (JNIEnv* env, jclass, jint device, jstring name)
-{
-  return HALSIM_GetSimValueHandle(device, JStringRef{env, name}.c_str());
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    enumerateSimValues
- * Signature: (I)[Ljava/lang/Object;
- */
-JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_enumerateSimValues
-  (JNIEnv* env, jclass, jint device)
-{
-  // get values
-  std::vector<ValueInfo> arr;
-  HALSIM_EnumerateSimValues(
-      device, &arr,
-      [](const char* name, void* param, HAL_SimValueHandle handle,
-         HAL_Bool readonly, const HAL_Value* value) {
-        auto arr = static_cast<std::vector<ValueInfo>*>(param);
-        arr->emplace_back(name, handle, readonly, *value);
-      });
-
-  // convert to java
-  size_t numElems = arr.size();
-  jobjectArray jarr = env->NewObjectArray(arr.size(), simValueInfoCls, nullptr);
-  if (!jarr) return nullptr;
-  for (size_t i = 0; i < numElems; ++i) {
-    JLocal<jobject> elem{env, arr[i].MakeJava(env)};
-    env->SetObjectArrayElement(jarr, i, elem.obj());
-  }
-  return jarr;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    getSimValueEnumOptions
- * Signature: (I)[Ljava/lang/Object;
- */
-JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_getSimValueEnumOptions
-  (JNIEnv* env, jclass, jint handle)
-{
-  static JClass stringCls{env, "java/lang/String"};
-  if (!stringCls) return nullptr;
-  int32_t numElems = 0;
-  const char** elems = HALSIM_GetSimValueEnumOptions(handle, &numElems);
-  jobjectArray jarr = env->NewObjectArray(numElems, stringCls, nullptr);
-  if (!jarr) return nullptr;
-  for (int32_t i = 0; i < numElems; ++i) {
-    JLocal<jstring> elem{env, MakeJString(env, elems[i])};
-    env->SetObjectArrayElement(jarr, i, elem.obj());
-  }
-  return jarr;
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI
- * Method:    resetSimDeviceData
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimDeviceDataJNI_resetSimDeviceData
-  (JNIEnv*, jclass)
-{
-  HALSIM_ResetSimDeviceData();
-}
-
-}  // extern "C"

hal/src/main/native/sim/jni/SimDeviceDataJNI.h

diff --git a/hal/src/main/native/sim/jni/SimDeviceDataJNI.h b/hal/src/main/native/sim/jni/SimDeviceDataJNI.h
deleted file mode 100644
index 56f6d9b..0000000
--- a/hal/src/main/native/sim/jni/SimDeviceDataJNI.h
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <jni.h>
-
-namespace sim {
-bool InitializeSimDeviceDataJNI(JNIEnv* env);
-void FreeSimDeviceDataJNI(JNIEnv* env);
-}  // namespace sim

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

diff --git a/hal/src/main/native/sim/jni/SimulatorJNI.cpp b/hal/src/main/native/sim/jni/SimulatorJNI.cpp
deleted file mode 100644
index 9226f91..0000000
--- a/hal/src/main/native/sim/jni/SimulatorJNI.cpp
+++ /dev/null
@@ -1,204 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "SimulatorJNI.h"
-
-#include <wpi/jni_util.h>
-
-#include "BufferCallbackStore.h"
-#include "CallbackStore.h"
-#include "ConstBufferCallbackStore.h"
-#include "SimDeviceDataJNI.h"
-#include "SpiReadAutoReceiveBufferCallbackStore.h"
-#include "edu_wpi_first_hal_sim_mockdata_SimulatorJNI.h"
-#include "hal/HAL.h"
-#include "hal/handles/HandlesInternal.h"
-#include "mockdata/MockHooks.h"
-
-using namespace wpi::java;
-
-static JavaVM* jvm = nullptr;
-static JClass notifyCallbackCls;
-static JClass bufferCallbackCls;
-static JClass constBufferCallbackCls;
-static JClass spiReadAutoReceiveBufferCallbackCls;
-static jmethodID notifyCallbackCallback;
-static jmethodID bufferCallbackCallback;
-static jmethodID constBufferCallbackCallback;
-static jmethodID spiReadAutoReceiveBufferCallbackCallback;
-
-namespace sim {
-jint SimOnLoad(JavaVM* vm, void* reserved) {
-  jvm = vm;
-
-  JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
-    return JNI_ERR;
-
-  notifyCallbackCls = JClass(env, "edu/wpi/first/hal/sim/NotifyCallback");
-  if (!notifyCallbackCls) return JNI_ERR;
-
-  notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative",
-                                            "(Ljava/lang/String;IJD)V");
-  if (!notifyCallbackCallback) return JNI_ERR;
-
-  bufferCallbackCls = JClass(env, "edu/wpi/first/hal/sim/BufferCallback");
-  if (!bufferCallbackCls) return JNI_ERR;
-
-  bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback",
-                                            "(Ljava/lang/String;[BI)V");
-  if (!bufferCallbackCallback) return JNI_ERR;
-
-  constBufferCallbackCls =
-      JClass(env, "edu/wpi/first/hal/sim/ConstBufferCallback");
-  if (!constBufferCallbackCls) return JNI_ERR;
-
-  constBufferCallbackCallback = env->GetMethodID(
-      constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V");
-  if (!constBufferCallbackCallback) return JNI_ERR;
-
-  spiReadAutoReceiveBufferCallbackCls =
-      JClass(env, "edu/wpi/first/hal/sim/SpiReadAutoReceiveBufferCallback");
-  if (!spiReadAutoReceiveBufferCallbackCls) return JNI_ERR;
-
-  spiReadAutoReceiveBufferCallbackCallback =
-      env->GetMethodID(spiReadAutoReceiveBufferCallbackCls, "callback",
-                       "(Ljava/lang/String;[II)I");
-  if (!spiReadAutoReceiveBufferCallbackCallback) return JNI_ERR;
-
-  InitializeStore();
-  InitializeBufferStore();
-  InitializeConstBufferStore();
-  InitializeSpiBufferStore();
-  if (!InitializeSimDeviceDataJNI(env)) return JNI_ERR;
-
-  return JNI_VERSION_1_6;
-}
-
-void SimOnUnload(JavaVM* vm, void* reserved) {
-  JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
-    return;
-
-  notifyCallbackCls.free(env);
-  bufferCallbackCls.free(env);
-  constBufferCallbackCls.free(env);
-  spiReadAutoReceiveBufferCallbackCls.free(env);
-  FreeSimDeviceDataJNI(env);
-  jvm = nullptr;
-}
-
-JavaVM* GetJVM() { return jvm; }
-
-jmethodID GetNotifyCallback() { return notifyCallbackCallback; }
-
-jmethodID GetBufferCallback() { return bufferCallbackCallback; }
-
-jmethodID GetConstBufferCallback() { return constBufferCallbackCallback; }
-
-jmethodID GetSpiReadAutoReceiveBufferCallback() {
-  return spiReadAutoReceiveBufferCallbackCallback;
-}
-}  // namespace sim
-
-extern "C" {
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    waitForProgramStart
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_waitForProgramStart
-  (JNIEnv*, jclass)
-{
-  HALSIM_WaitForProgramStart();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    setProgramStarted
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_setProgramStarted
-  (JNIEnv*, jclass)
-{
-  HALSIM_SetProgramStarted();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    restartTiming
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_restartTiming
-  (JNIEnv*, jclass)
-{
-  HALSIM_RestartTiming();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    pauseTiming
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_pauseTiming
-  (JNIEnv*, jclass)
-{
-  HALSIM_PauseTiming();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    resumeTiming
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_resumeTiming
-  (JNIEnv*, jclass)
-{
-  HALSIM_ResumeTiming();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    isTimingPaused
- * Signature: ()Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_isTimingPaused
-  (JNIEnv*, jclass)
-{
-  return HALSIM_IsTimingPaused();
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    stepTiming
- * Signature: (J)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_stepTiming
-  (JNIEnv*, jclass, jlong delta)
-{
-  HALSIM_StepTiming(delta);
-}
-
-/*
- * Class:     edu_wpi_first_hal_sim_mockdata_SimulatorJNI
- * Method:    resetHandles
- * Signature: ()V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_resetHandles
-  (JNIEnv*, jclass)
-{
-  hal::HandleBase::ResetGlobalHandles();
-}
-}  // extern "C"

hal/src/main/native/sim/jni/SimulatorJNI.h

diff --git a/hal/src/main/native/sim/jni/SimulatorJNI.h b/hal/src/main/native/sim/jni/SimulatorJNI.h
deleted file mode 100644
index 8680396..0000000
--- a/hal/src/main/native/sim/jni/SimulatorJNI.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "hal/Types.h"
-#include "jni.h"
-
-typedef HAL_Handle SIM_JniHandle;
-
-namespace sim {
-JavaVM* GetJVM();
-
-jmethodID GetNotifyCallback();
-jmethodID GetBufferCallback();
-jmethodID GetConstBufferCallback();
-jmethodID GetSpiReadAutoReceiveBufferCallback();
-}  // namespace sim

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

diff --git a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp
deleted file mode 100644
index b75bb1e..0000000
--- a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.cpp
+++ /dev/null
@@ -1,130 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "SpiReadAutoReceiveBufferCallbackStore.h"
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-
-using namespace wpi::java;
-using namespace sim;
-
-static hal::UnlimitedHandleResource<
-    SIM_JniHandle, SpiReadAutoReceiveBufferCallbackStore,
-    hal::HAL_HandleEnum::SimulationJni>* callbackHandles;
-
-namespace sim {
-void InitializeSpiBufferStore() {
-  static hal::UnlimitedHandleResource<SIM_JniHandle,
-                                      SpiReadAutoReceiveBufferCallbackStore,
-                                      hal::HAL_HandleEnum::SimulationJni>
-      cb;
-  callbackHandles = &cb;
-}
-}  // namespace sim
-
-void SpiReadAutoReceiveBufferCallbackStore::create(JNIEnv* env, jobject obj) {
-  m_call = JGlobal<jobject>(env, obj);
-}
-
-int32_t SpiReadAutoReceiveBufferCallbackStore::performCallback(
-    const char* name, uint32_t* buffer, int32_t numToRead) {
-  JNIEnv* env;
-  JavaVM* vm = sim::GetJVM();
-  bool didAttachThread = false;
-  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
-  if (tryGetEnv == JNI_EDETACHED) {
-    // Thread not attached
-    didAttachThread = true;
-    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
-      // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
-      return -1;
-    }
-  } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
-  }
-
-  auto toCallbackArr = MakeJIntArray(
-      env, wpi::ArrayRef<uint32_t>{buffer, static_cast<size_t>(numToRead)});
-
-  jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(),
-                                MakeJString(env, name), toCallbackArr,
-                                (jint)numToRead);
-
-  jint* fromCallbackArr = reinterpret_cast<jint*>(
-      env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
-
-  for (int i = 0; i < ret; i++) {
-    buffer[i] = fromCallbackArr[i];
-  }
-
-  env->ReleasePrimitiveArrayCritical(toCallbackArr, fromCallbackArr, JNI_ABORT);
-
-  if (env->ExceptionCheck()) {
-    env->ExceptionDescribe();
-  }
-
-  if (didAttachThread) {
-    vm->DetachCurrentThread();
-  }
-  return ret;
-}
-
-void SpiReadAutoReceiveBufferCallbackStore::free(JNIEnv* env) {
-  m_call.free(env);
-}
-
-SIM_JniHandle sim::AllocateSpiBufferCallback(
-    JNIEnv* env, jint index, jobject callback,
-    RegisterSpiBufferCallbackFunc createCallback) {
-  auto callbackStore =
-      std::make_shared<SpiReadAutoReceiveBufferCallbackStore>();
-
-  auto handle = callbackHandles->Allocate(callbackStore);
-
-  if (handle == HAL_kInvalidHandle) {
-    return -1;
-  }
-
-  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
-  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
-
-  callbackStore->create(env, callback);
-
-  auto callbackFunc = [](const char* name, void* param, uint32_t* buffer,
-                         int32_t numToRead, int32_t* outputCount) {
-    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-    SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
-    auto data = callbackHandles->Get(handle);
-    if (!data) return;
-
-    *outputCount = data->performCallback(name, buffer, numToRead);
-  };
-
-  auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
-
-  callbackStore->setCallbackId(id);
-
-  return handle;
-}
-
-void sim::FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                                FreeSpiBufferCallbackFunc freeCallback) {
-  auto callback = callbackHandles->Free(handle);
-  freeCallback(index, callback->getCallbackId());
-  callback->free(env);
-}

hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h

diff --git a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h b/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h
deleted file mode 100644
index 1a03f59..0000000
--- a/hal/src/main/native/sim/jni/SpiReadAutoReceiveBufferCallbackStore.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <jni.h>
-
-#include <wpi/jni_util.h>
-
-#include "SimulatorJNI.h"
-#include "hal/Types.h"
-#include "hal/Value.h"
-#include "hal/handles/UnlimitedHandleResource.h"
-#include "mockdata/NotifyListener.h"
-#include "mockdata/SPIData.h"
-
-namespace sim {
-class SpiReadAutoReceiveBufferCallbackStore {
- public:
-  void create(JNIEnv* env, jobject obj);
-  int32_t performCallback(const char* name, uint32_t* buffer,
-                          int32_t numToRead);
-  void free(JNIEnv* env);
-  void setCallbackId(int32_t id) { callbackId = id; }
-  int32_t getCallbackId() { return callbackId; }
-
- private:
-  wpi::java::JGlobal<jobject> m_call;
-  int32_t callbackId;
-};
-
-void InitializeSpiBufferStore();
-
-typedef int32_t (*RegisterSpiBufferCallbackFunc)(
-    int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
-typedef void (*FreeSpiBufferCallbackFunc)(int32_t index, int32_t uid);
-
-SIM_JniHandle AllocateSpiBufferCallback(
-    JNIEnv* env, jint index, jobject callback,
-    RegisterSpiBufferCallbackFunc createCallback);
-void FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
-                           FreeSpiBufferCallbackFunc freeCallback);
-}  // namespace sim

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

diff --git a/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h b/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
index 9db0875..e2e0a80 100644
--- a/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/AccelerometerData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/AccelerometerData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class AccelerometerData {

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

diff --git a/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp b/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
index 5b44eaf..fe4611e 100644
--- a/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
+++ b/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -49,6 +49,16 @@
 }
 
 extern "C" {
+
+int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) {
+  for (int i = 0; i < kNumAddressableLEDs; ++i) {
+    if (SimAddressableLEDData[i].initialized &&
+        SimAddressableLEDData[i].outputPort == channel)
+      return i;
+  }
+  return -1;
+}
+
 void HALSIM_ResetAddressableLEDData(int32_t index) {
   SimAddressableLEDData[index].ResetData();
 }

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

diff --git a/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h b/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
index 9d6e215..f0b7998 100644
--- a/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,9 +11,9 @@
 
 #include <wpi/spinlock.h>
 
-#include "mockdata/AddressableLEDData.h"
-#include "mockdata/SimCallbackRegistry.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/AddressableLEDData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class AddressableLEDData {

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

diff --git a/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
index 427d2fb..ff12b60 100644
--- a/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/AnalogGyroData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/AnalogGyroData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class AnalogGyroData {

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

diff --git a/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
index cd8348d..90d707c 100644
--- a/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/AnalogInData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/AnalogInData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class AnalogInData {

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

diff --git a/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
index da7d49f..779ee96 100644
--- a/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/AnalogOutData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/AnalogOutData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class AnalogOutData {

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

diff --git a/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp b/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
index 64d1a97..ba33522 100644
--- a/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
+++ b/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -28,6 +28,16 @@
 }
 
 extern "C" {
+
+int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel) {
+  for (int i = 0; i < kNumAnalogTriggers; ++i) {
+    if (SimAnalogTriggerData[i].initialized &&
+        SimAnalogTriggerData[i].inputPort == channel)
+      return i;
+  }
+  return -1;
+}
+
 void HALSIM_ResetAnalogTriggerData(int32_t index) {
   SimAnalogTriggerData[index].ResetData();
 }

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

diff --git a/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h b/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
index 61b3c19..74fe5f8 100644
--- a/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/AnalogTriggerData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/AnalogTriggerData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class AnalogTriggerData {
@@ -31,6 +31,7 @@
   SimDataValue<HALSIM_AnalogTriggerMode, MakeTriggerModeValue,
                GetTriggerModeName>
       triggerMode{static_cast<HALSIM_AnalogTriggerMode>(0)};
+  std::atomic<int32_t> inputPort;
 
   virtual void ResetData();
 };

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

diff --git a/hal/src/main/native/sim/mockdata/CanDataInternal.h b/hal/src/main/native/sim/mockdata/CanDataInternal.h
index ffdd351..df5b290 100644
--- a/hal/src/main/native/sim/mockdata/CanDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/CanDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/CanData.h"
-#include "mockdata/SimCallbackRegistry.h"
+#include "hal/simulation/CanData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
 
 namespace hal {
 

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

diff --git a/hal/src/main/native/sim/mockdata/DIODataInternal.h b/hal/src/main/native/sim/mockdata/DIODataInternal.h
index 49e0f75..c3266d5 100644
--- a/hal/src/main/native/sim/mockdata/DIODataInternal.h
+++ b/hal/src/main/native/sim/mockdata/DIODataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/DIOData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/DIOData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class DIOData {

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

diff --git a/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp b/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
index 78ee749..541d7cf 100644
--- a/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
+++ b/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -27,6 +27,14 @@
 }
 
 extern "C" {
+int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel) {
+  for (int i = 0; i < kNumDigitalPWMOutputs; ++i) {
+    if (SimDigitalPWMData[i].initialized && SimDigitalPWMData[i].pin == channel)
+      return i;
+  }
+  return -1;
+}
+
 void HALSIM_ResetDigitalPWMData(int32_t index) {
   SimDigitalPWMData[index].ResetData();
 }

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

diff --git a/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h b/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
index 09d5903..f2c7a5e 100644
--- a/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/DigitalPWMData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/DigitalPWMData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class DigitalPWMData {

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

diff --git a/hal/src/main/native/sim/mockdata/DriverStationData.cpp b/hal/src/main/native/sim/mockdata/DriverStationData.cpp
index ece98c8..dae1dd4 100644
--- a/hal/src/main/native/sim/mockdata/DriverStationData.cpp
+++ b/hal/src/main/native/sim/mockdata/DriverStationData.cpp
@@ -1,25 +1,15 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <cstdlib>
 #include <cstring>
-#include <string>
 
 #include "DriverStationDataInternal.h"
 #include "hal/DriverStation.h"
 
-namespace hal {
-struct JoystickOutputStore {
-  int64_t outputs = 0;
-  int32_t leftRumble = 0;
-  int32_t rightRumble = 0;
-};
-}  // namespace hal
-
 using namespace hal;
 
 namespace hal {
@@ -47,103 +37,322 @@
 
   {
     std::scoped_lock lock(m_joystickDataMutex);
-    m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(6);
-    m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(6);
-    m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(6);
-    m_joystickOutputs = std::make_unique<JoystickOutputStore[]>(6);
-    m_joystickDescriptor = std::make_unique<HAL_JoystickDescriptor[]>(6);
-
-    for (int i = 0; i < 6; i++) {
-      m_joystickAxes[i].count = 0;
-      m_joystickPOVs[i].count = 0;
-      m_joystickButtons[i].count = 0;
-      m_joystickDescriptor[i].isXbox = 0;
-      m_joystickDescriptor[i].type = -1;
-      m_joystickDescriptor[i].name[0] = '\0';
+    m_joystickAxesCallbacks.Reset();
+    m_joystickPOVsCallbacks.Reset();
+    m_joystickButtonsCallbacks.Reset();
+    m_joystickOutputsCallbacks.Reset();
+    m_joystickDescriptorCallbacks.Reset();
+    for (int i = 0; i < kNumJoysticks; i++) {
+      m_joystickData[i].axes = HAL_JoystickAxes{};
+      m_joystickData[i].povs = HAL_JoystickPOVs{};
+      m_joystickData[i].buttons = HAL_JoystickButtons{};
+      m_joystickData[i].descriptor = HAL_JoystickDescriptor{};
+      m_joystickData[i].descriptor.type = -1;
+      m_joystickData[i].descriptor.name[0] = '\0';
     }
   }
   {
     std::scoped_lock lock(m_matchInfoMutex);
-
-    m_matchInfo = std::make_unique<HAL_MatchInfo>();
+    m_matchInfoCallbacks.Reset();
+    m_matchInfo = HAL_MatchInfo{};
   }
+  m_newDataCallbacks.Reset();
 }
 
-void DriverStationData::GetJoystickAxes(int32_t joystickNum,
-                                        HAL_JoystickAxes* axes) {
-  std::scoped_lock lock(m_joystickDataMutex);
-  *axes = m_joystickAxes[joystickNum];
-}
-void DriverStationData::GetJoystickPOVs(int32_t joystickNum,
-                                        HAL_JoystickPOVs* povs) {
-  std::scoped_lock lock(m_joystickDataMutex);
-  *povs = m_joystickPOVs[joystickNum];
-}
-void DriverStationData::GetJoystickButtons(int32_t joystickNum,
-                                           HAL_JoystickButtons* buttons) {
-  std::scoped_lock lock(m_joystickDataMutex);
-  *buttons = m_joystickButtons[joystickNum];
-}
+#define DEFINE_CPPAPI_CALLBACKS(name, data, data2)                             \
+  int32_t DriverStationData::RegisterJoystick##name##Callback(                 \
+      int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
+      HAL_Bool initialNotify) {                                                \
+    if (joystickNum < 0 || joystickNum >= kNumJoysticks) return 0;             \
+    std::scoped_lock lock(m_joystickDataMutex);                                \
+    int32_t uid = m_joystick##name##Callbacks.Register(callback, param);       \
+    if (initialNotify) {                                                       \
+      callback(GetJoystick##name##Name(), param, joystickNum,                  \
+               &m_joystickData[joystickNum].data##data2);                      \
+    }                                                                          \
+    return uid;                                                                \
+  }                                                                            \
+                                                                               \
+  void DriverStationData::CancelJoystick##name##Callback(int32_t uid) {        \
+    m_joystick##name##Callbacks.Cancel(uid);                                   \
+  }
+
+#define DEFINE_CPPAPI(name, data, data2)                                   \
+  DEFINE_CPPAPI_CALLBACKS(name, data, data2)                               \
+                                                                           \
+  void DriverStationData::GetJoystick##name(int32_t joystickNum,           \
+                                            HAL_Joystick##name* d) {       \
+    if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;           \
+    std::scoped_lock lock(m_joystickDataMutex);                            \
+    *d = m_joystickData[joystickNum].data##data2;                          \
+  }                                                                        \
+                                                                           \
+  void DriverStationData::SetJoystick##name(int32_t joystickNum,           \
+                                            const HAL_Joystick##name* d) { \
+    if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;           \
+    std::scoped_lock lock(m_joystickDataMutex);                            \
+    m_joystickData[joystickNum].data##data2 = *d;                          \
+    m_joystick##name##Callbacks(joystickNum, d);                           \
+  }
+
+DEFINE_CPPAPI(Axes, axes, )
+DEFINE_CPPAPI(POVs, povs, )
+DEFINE_CPPAPI(Buttons, buttons, )
+
+DEFINE_CPPAPI_CALLBACKS(Descriptor, descriptor, )
+
 void DriverStationData::GetJoystickDescriptor(
     int32_t joystickNum, HAL_JoystickDescriptor* descriptor) {
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
   std::scoped_lock lock(m_joystickDataMutex);
-  *descriptor = m_joystickDescriptor[joystickNum];
-  // Always ensure name is null terminated
-  descriptor->name[255] = '\0';
-}
-void DriverStationData::GetJoystickOutputs(int32_t joystickNum,
-                                           int64_t* outputs,
-                                           int32_t* leftRumble,
-                                           int32_t* rightRumble) {
-  std::scoped_lock lock(m_joystickDataMutex);
-  *leftRumble = m_joystickOutputs[joystickNum].leftRumble;
-  *outputs = m_joystickOutputs[joystickNum].outputs;
-  *rightRumble = m_joystickOutputs[joystickNum].rightRumble;
-}
-void DriverStationData::GetMatchInfo(HAL_MatchInfo* info) {
-  std::scoped_lock lock(m_matchInfoMutex);
-  *info = *m_matchInfo;
-}
-
-void DriverStationData::SetJoystickAxes(int32_t joystickNum,
-                                        const HAL_JoystickAxes* axes) {
-  std::scoped_lock lock(m_joystickDataMutex);
-  m_joystickAxes[joystickNum] = *axes;
-}
-void DriverStationData::SetJoystickPOVs(int32_t joystickNum,
-                                        const HAL_JoystickPOVs* povs) {
-  std::scoped_lock lock(m_joystickDataMutex);
-  m_joystickPOVs[joystickNum] = *povs;
-}
-void DriverStationData::SetJoystickButtons(int32_t joystickNum,
-                                           const HAL_JoystickButtons* buttons) {
-  std::scoped_lock lock(m_joystickDataMutex);
-  m_joystickButtons[joystickNum] = *buttons;
+  *descriptor = m_joystickData[joystickNum].descriptor;
 }
 
 void DriverStationData::SetJoystickDescriptor(
     int32_t joystickNum, const HAL_JoystickDescriptor* descriptor) {
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
   std::scoped_lock lock(m_joystickDataMutex);
-  m_joystickDescriptor[joystickNum] = *descriptor;
+  m_joystickData[joystickNum].descriptor = *descriptor;
+  // Always ensure name is null terminated
+  m_joystickData[joystickNum].descriptor.name[255] = '\0';
+  m_joystickDescriptorCallbacks(joystickNum, descriptor);
+}
+
+int32_t DriverStationData::RegisterJoystickOutputsCallback(
+    int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
+    HAL_Bool initialNotify) {
+  if (joystickNum < 0 || joystickNum >= DriverStationData::kNumJoysticks)
+    return 0;
+  std::scoped_lock lock(m_joystickDataMutex);
+  int32_t uid = m_joystickOutputsCallbacks.Register(callback, param);
+  if (initialNotify) {
+    const auto& outputs = m_joystickData[joystickNum].outputs;
+    callback(DriverStationData::GetJoystickOutputsName(), param, joystickNum,
+             outputs.outputs, outputs.leftRumble, outputs.rightRumble);
+  }
+  return uid;
+}
+
+void DriverStationData::CancelJoystickOutputsCallback(int32_t uid) {
+  m_joystickOutputsCallbacks.Cancel(uid);
+}
+
+void DriverStationData::GetJoystickOutputs(int32_t joystickNum,
+                                           int64_t* outputs,
+                                           int32_t* leftRumble,
+                                           int32_t* rightRumble) {
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  *leftRumble = m_joystickData[joystickNum].outputs.leftRumble;
+  *outputs = m_joystickData[joystickNum].outputs.outputs;
+  *rightRumble = m_joystickData[joystickNum].outputs.rightRumble;
 }
 
 void DriverStationData::SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
                                            int32_t leftRumble,
                                            int32_t rightRumble) {
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
   std::scoped_lock lock(m_joystickDataMutex);
-  m_joystickOutputs[joystickNum].leftRumble = leftRumble;
-  m_joystickOutputs[joystickNum].outputs = outputs;
-  m_joystickOutputs[joystickNum].rightRumble = rightRumble;
+  m_joystickData[joystickNum].outputs.leftRumble = leftRumble;
+  m_joystickData[joystickNum].outputs.outputs = outputs;
+  m_joystickData[joystickNum].outputs.rightRumble = rightRumble;
+  m_joystickOutputsCallbacks(joystickNum, outputs, leftRumble, rightRumble);
+}
+
+int32_t DriverStationData::RegisterMatchInfoCallback(
+    HAL_MatchInfoCallback callback, void* param, HAL_Bool initialNotify) {
+  std::scoped_lock lock(m_matchInfoMutex);
+  int32_t uid = m_matchInfoCallbacks.Register(callback, param);
+  if (initialNotify) {
+    callback(GetMatchInfoName(), param, &m_matchInfo);
+  }
+  return uid;
+}
+
+void DriverStationData::CancelMatchInfoCallback(int32_t uid) {
+  m_matchInfoCallbacks.Cancel(uid);
+}
+
+void DriverStationData::GetMatchInfo(HAL_MatchInfo* info) {
+  std::scoped_lock lock(m_matchInfoMutex);
+  *info = m_matchInfo;
 }
 
 void DriverStationData::SetMatchInfo(const HAL_MatchInfo* info) {
   std::scoped_lock lock(m_matchInfoMutex);
-  *m_matchInfo = *info;
-  *(std::end(m_matchInfo->eventName) - 1) = '\0';
+  m_matchInfo = *info;
+  *(std::end(m_matchInfo.eventName) - 1) = '\0';
+  m_matchInfoCallbacks(info);
+}
+
+int32_t DriverStationData::RegisterNewDataCallback(HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify) {
+  int32_t uid = m_newDataCallbacks.Register(callback, param);
+  if (initialNotify) {
+    HAL_Value empty;
+    empty.type = HAL_UNASSIGNED;
+    callback(GetNewDataName(), param, &empty);
+  }
+  return uid;
+}
+
+void DriverStationData::CancelNewDataCallback(int32_t uid) {
+  m_newDataCallbacks.Cancel(uid);
+}
+
+void DriverStationData::CallNewDataCallbacks() {
+  HAL_Value empty;
+  empty.type = HAL_UNASSIGNED;
+  m_newDataCallbacks(&empty);
 }
 
 void DriverStationData::NotifyNewData() { HAL_ReleaseDSMutex(); }
 
+void DriverStationData::SetJoystickButton(int32_t stick, int32_t button,
+                                          HAL_Bool state) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  if (state)
+    m_joystickData[stick].buttons.buttons |= 1 << (button - 1);
+  else
+    m_joystickData[stick].buttons.buttons &= ~(1 << (button - 1));
+  m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
+}
+
+void DriverStationData::SetJoystickAxis(int32_t stick, int32_t axis,
+                                        double value) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].axes.axes[axis] = value;
+  m_joystickAxesCallbacks(stick, &m_joystickData[stick].axes);
+}
+
+void DriverStationData::SetJoystickPOV(int32_t stick, int32_t pov,
+                                       int32_t value) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (pov < 0 || pov >= HAL_kMaxJoystickPOVs) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].povs.povs[pov] = value;
+  m_joystickPOVsCallbacks(stick, &m_joystickData[stick].povs);
+}
+
+void DriverStationData::SetJoystickButtons(int32_t stick, uint32_t buttons) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].buttons.buttons = buttons;
+  m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
+}
+
+void DriverStationData::SetJoystickAxisCount(int32_t stick, int32_t count) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].axes.count = count;
+  m_joystickData[stick].descriptor.axisCount = count;
+  m_joystickAxesCallbacks(stick, &m_joystickData[stick].axes);
+  m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
+}
+
+void DriverStationData::SetJoystickPOVCount(int32_t stick, int32_t count) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].povs.count = count;
+  m_joystickData[stick].descriptor.povCount = count;
+  m_joystickPOVsCallbacks(stick, &m_joystickData[stick].povs);
+  m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
+}
+
+void DriverStationData::SetJoystickButtonCount(int32_t stick, int32_t count) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].buttons.count = count;
+  m_joystickData[stick].descriptor.buttonCount = count;
+  m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
+  m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
+}
+
+void DriverStationData::GetJoystickCounts(int32_t stick, int32_t* axisCount,
+                                          int32_t* buttonCount,
+                                          int32_t* povCount) {
+  if (stick < 0 || stick >= kNumJoysticks) {
+    *axisCount = 0;
+    *buttonCount = 0;
+    *povCount = 0;
+    return;
+  }
+  std::scoped_lock lock(m_joystickDataMutex);
+  *axisCount = m_joystickData[stick].axes.count;
+  *buttonCount = m_joystickData[stick].buttons.count;
+  *povCount = m_joystickData[stick].povs.count;
+}
+
+void DriverStationData::SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].descriptor.isXbox = isXbox;
+  m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
+}
+
+void DriverStationData::SetJoystickType(int32_t stick, int32_t type) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].descriptor.type = type;
+  m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
+}
+
+void DriverStationData::SetJoystickName(int32_t stick, const char* name) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  std::strncpy(m_joystickData[stick].descriptor.name, name,
+               sizeof(m_joystickData[stick].descriptor.name) - 1);
+  *(std::end(m_joystickData[stick].descriptor.name) - 1) = '\0';
+  m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
+}
+
+void DriverStationData::SetJoystickAxisType(int32_t stick, int32_t axis,
+                                            int32_t type) {
+  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) return;
+  std::scoped_lock lock(m_joystickDataMutex);
+  m_joystickData[stick].descriptor.axisTypes[axis] = type;
+  m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
+}
+
+void DriverStationData::SetGameSpecificMessage(const char* message) {
+  std::scoped_lock lock(m_matchInfoMutex);
+  std::strncpy(reinterpret_cast<char*>(m_matchInfo.gameSpecificMessage),
+               message, sizeof(m_matchInfo.gameSpecificMessage) - 1);
+  *(std::end(m_matchInfo.gameSpecificMessage) - 1) = '\0';
+  m_matchInfo.gameSpecificMessageSize = std::strlen(message);
+  m_matchInfoCallbacks(&m_matchInfo);
+}
+
+void DriverStationData::SetEventName(const char* name) {
+  std::scoped_lock lock(m_matchInfoMutex);
+  std::strncpy(m_matchInfo.eventName, name, sizeof(m_matchInfo.eventName) - 1);
+  *(std::end(m_matchInfo.eventName) - 1) = '\0';
+  m_matchInfoCallbacks(&m_matchInfo);
+}
+
+void DriverStationData::SetMatchType(HAL_MatchType type) {
+  std::scoped_lock lock(m_matchInfoMutex);
+  m_matchInfo.matchType = type;
+  m_matchInfoCallbacks(&m_matchInfo);
+}
+
+void DriverStationData::SetMatchNumber(int32_t matchNumber) {
+  std::scoped_lock lock(m_matchInfoMutex);
+  m_matchInfo.matchNumber = matchNumber;
+  m_matchInfoCallbacks(&m_matchInfo);
+}
+
+void DriverStationData::SetReplayNumber(int32_t replayNumber) {
+  std::scoped_lock lock(m_matchInfoMutex);
+  m_matchInfo.replayNumber = replayNumber;
+  m_matchInfoCallbacks(&m_matchInfo);
+}
+
 extern "C" {
 void HALSIM_ResetDriverStationData(void) { SimDriverStationData->ResetData(); }
 
@@ -160,21 +369,42 @@
 DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, allianceStationId)
 DEFINE_CAPI(double, MatchTime, matchTime)
 
-void HALSIM_SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes) {
-  SimDriverStationData->SetJoystickAxes(joystickNum, axes);
+#undef DEFINE_CAPI
+#define DEFINE_CAPI(name, data)                                                \
+  int32_t HALSIM_RegisterJoystick##name##Callback(                             \
+      int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
+      HAL_Bool initialNotify) {                                                \
+    return SimDriverStationData->RegisterJoystick##name##Callback(             \
+        joystickNum, callback, param, initialNotify);                          \
+  }                                                                            \
+                                                                               \
+  void HALSIM_CancelJoystick##name##Callback(int32_t uid) {                    \
+    SimDriverStationData->CancelJoystick##name##Callback(uid);                 \
+  }                                                                            \
+                                                                               \
+  void HALSIM_GetJoystick##name(int32_t joystickNum, HAL_Joystick##name* d) {  \
+    SimDriverStationData->GetJoystick##name(joystickNum, d);                   \
+  }                                                                            \
+                                                                               \
+  void HALSIM_SetJoystick##name(int32_t joystickNum,                           \
+                                const HAL_Joystick##name* d) {                 \
+    SimDriverStationData->SetJoystick##name(joystickNum, d);                   \
+  }
+
+DEFINE_CAPI(Axes, axes)
+DEFINE_CAPI(POVs, povs)
+DEFINE_CAPI(Buttons, buttons)
+DEFINE_CAPI(Descriptor, descriptor)
+
+int32_t HALSIM_RegisterJoystickOutputsCallback(
+    int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
+    HAL_Bool initialNotify) {
+  return SimDriverStationData->RegisterJoystickOutputsCallback(
+      joystickNum, callback, param, initialNotify);
 }
 
-void HALSIM_SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs) {
-  SimDriverStationData->SetJoystickPOVs(joystickNum, povs);
-}
-
-void HALSIM_SetJoystickButtons(int32_t joystickNum,
-                               const HAL_JoystickButtons* buttons) {
-  SimDriverStationData->SetJoystickButtons(joystickNum, buttons);
-}
-void HALSIM_SetJoystickDescriptor(int32_t joystickNum,
-                                  const HAL_JoystickDescriptor* descriptor) {
-  SimDriverStationData->SetJoystickDescriptor(joystickNum, descriptor);
+void HALSIM_CancelJoystickOutputsCallback(int32_t uid) {
+  SimDriverStationData->CancelJoystickOutputsCallback(uid);
 }
 
 void HALSIM_GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
@@ -183,14 +413,115 @@
                                            rightRumble);
 }
 
+void HALSIM_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
+                               int32_t leftRumble, int32_t rightRumble) {
+  SimDriverStationData->SetJoystickOutputs(joystickNum, outputs, leftRumble,
+                                           rightRumble);
+}
+
+int32_t HALSIM_RegisterMatchInfoCallback(HAL_MatchInfoCallback callback,
+                                         void* param, HAL_Bool initialNotify) {
+  return SimDriverStationData->RegisterMatchInfoCallback(callback, param,
+                                                         initialNotify);
+}
+
+void HALSIM_CancelMatchInfoCallback(int32_t uid) {
+  SimDriverStationData->CancelMatchInfoCallback(uid);
+}
+
+void HALSIM_GetMatchInfo(HAL_MatchInfo* info) {
+  SimDriverStationData->GetMatchInfo(info);
+}
+
 void HALSIM_SetMatchInfo(const HAL_MatchInfo* info) {
   SimDriverStationData->SetMatchInfo(info);
 }
 
+int32_t HALSIM_RegisterDriverStationNewDataCallback(HAL_NotifyCallback callback,
+                                                    void* param,
+                                                    HAL_Bool initialNotify) {
+  return SimDriverStationData->RegisterNewDataCallback(callback, param,
+                                                       initialNotify);
+}
+
+void HALSIM_CancelDriverStationNewDataCallback(int32_t uid) {
+  SimDriverStationData->CancelNewDataCallback(uid);
+}
+
 void HALSIM_NotifyDriverStationNewData(void) {
   SimDriverStationData->NotifyNewData();
 }
 
+void HALSIM_SetJoystickButton(int32_t stick, int32_t button, HAL_Bool state) {
+  SimDriverStationData->SetJoystickButton(stick, button, state);
+}
+
+void HALSIM_SetJoystickAxis(int32_t stick, int32_t axis, double value) {
+  SimDriverStationData->SetJoystickAxis(stick, axis, value);
+}
+
+void HALSIM_SetJoystickPOV(int32_t stick, int32_t pov, int32_t value) {
+  SimDriverStationData->SetJoystickPOV(stick, pov, value);
+}
+
+void HALSIM_SetJoystickButtonsValue(int32_t stick, uint32_t buttons) {
+  SimDriverStationData->SetJoystickButtons(stick, buttons);
+}
+
+void HALSIM_SetJoystickAxisCount(int32_t stick, int32_t count) {
+  SimDriverStationData->SetJoystickAxisCount(stick, count);
+}
+
+void HALSIM_SetJoystickPOVCount(int32_t stick, int32_t count) {
+  SimDriverStationData->SetJoystickPOVCount(stick, count);
+}
+
+void HALSIM_SetJoystickButtonCount(int32_t stick, int32_t count) {
+  SimDriverStationData->SetJoystickButtonCount(stick, count);
+}
+
+void HALSIM_GetJoystickCounts(int32_t stick, int32_t* axisCount,
+                              int32_t* buttonCount, int32_t* povCount) {
+  SimDriverStationData->GetJoystickCounts(stick, axisCount, buttonCount,
+                                          povCount);
+}
+
+void HALSIM_SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {
+  SimDriverStationData->SetJoystickIsXbox(stick, isXbox);
+}
+
+void HALSIM_SetJoystickType(int32_t stick, int32_t type) {
+  SimDriverStationData->SetJoystickType(stick, type);
+}
+
+void HALSIM_SetJoystickName(int32_t stick, const char* name) {
+  SimDriverStationData->SetJoystickName(stick, name);
+}
+
+void HALSIM_SetJoystickAxisType(int32_t stick, int32_t axis, int32_t type) {
+  SimDriverStationData->SetJoystickAxisType(stick, axis, type);
+}
+
+void HALSIM_SetGameSpecificMessage(const char* message) {
+  SimDriverStationData->SetGameSpecificMessage(message);
+}
+
+void HALSIM_SetEventName(const char* name) {
+  SimDriverStationData->SetEventName(name);
+}
+
+void HALSIM_SetMatchType(HAL_MatchType type) {
+  SimDriverStationData->SetMatchType(type);
+}
+
+void HALSIM_SetMatchNumber(int32_t matchNumber) {
+  SimDriverStationData->SetMatchNumber(matchNumber);
+}
+
+void HALSIM_SetReplayNumber(int32_t replayNumber) {
+  SimDriverStationData->SetReplayNumber(replayNumber);
+}
+
 #define REGISTER(NAME) \
   SimDriverStationData->NAME.RegisterCallback(callback, param, initialNotify)
 

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

diff --git a/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
index fdeb3c6..2a50742 100644
--- a/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,11 +11,11 @@
 
 #include <wpi/spinlock.h>
 
-#include "mockdata/DriverStationData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/DriverStationData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
-struct JoystickOutputStore;
 
 class DriverStationData {
   HAL_SIMDATAVALUE_DEFINE_NAME(Enabled)
@@ -26,6 +26,13 @@
   HAL_SIMDATAVALUE_DEFINE_NAME(DsAttached)
   HAL_SIMDATAVALUE_DEFINE_NAME(AllianceStationId)
   HAL_SIMDATAVALUE_DEFINE_NAME(MatchTime)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickAxes)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickPOVs)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickButtons)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickDescriptor)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickOutputs)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(MatchInfo)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(NewData)
 
   static LLVM_ATTRIBUTE_ALWAYS_INLINE HAL_Value
   MakeAllianceStationIdValue(HAL_AllianceStationID value) {
@@ -36,28 +43,82 @@
   DriverStationData();
   void ResetData();
 
+  int32_t RegisterJoystickAxesCallback(int32_t joystickNum,
+                                       HAL_JoystickAxesCallback callback,
+                                       void* param, HAL_Bool initialNotify);
+  void CancelJoystickAxesCallback(int32_t uid);
   void GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes);
-  void GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs);
-  void GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons);
-  void GetJoystickDescriptor(int32_t joystickNum,
-                             HAL_JoystickDescriptor* descriptor);
-  void GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
-                          int32_t* leftRumble, int32_t* rightRumble);
-  void GetMatchInfo(HAL_MatchInfo* info);
-  void FreeMatchInfo(const HAL_MatchInfo* info);
-
   void SetJoystickAxes(int32_t joystickNum, const HAL_JoystickAxes* axes);
+
+  int32_t RegisterJoystickPOVsCallback(int32_t joystickNum,
+                                       HAL_JoystickPOVsCallback callback,
+                                       void* param, HAL_Bool initialNotify);
+  void CancelJoystickPOVsCallback(int32_t uid);
+  void GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs);
   void SetJoystickPOVs(int32_t joystickNum, const HAL_JoystickPOVs* povs);
+
+  int32_t RegisterJoystickButtonsCallback(int32_t joystickNum,
+                                          HAL_JoystickButtonsCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+  void CancelJoystickButtonsCallback(int32_t uid);
+  void GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons);
   void SetJoystickButtons(int32_t joystickNum,
                           const HAL_JoystickButtons* buttons);
+
+  int32_t RegisterJoystickDescriptorCallback(
+      int32_t joystickNum, HAL_JoystickDescriptorCallback callback, void* param,
+      HAL_Bool initialNotify);
+  void CancelJoystickDescriptorCallback(int32_t uid);
+  void GetJoystickDescriptor(int32_t joystickNum,
+                             HAL_JoystickDescriptor* descriptor);
   void SetJoystickDescriptor(int32_t joystickNum,
                              const HAL_JoystickDescriptor* descriptor);
+
+  int32_t RegisterJoystickOutputsCallback(int32_t joystickNum,
+                                          HAL_JoystickOutputsCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+  void CancelJoystickOutputsCallback(int32_t uid);
+  void GetJoystickOutputs(int32_t joystickNum, int64_t* outputs,
+                          int32_t* leftRumble, int32_t* rightRumble);
   void SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
                           int32_t leftRumble, int32_t rightRumble);
+
+  int32_t RegisterMatchInfoCallback(HAL_MatchInfoCallback callback, void* param,
+                                    HAL_Bool initialNotify);
+  void CancelMatchInfoCallback(int32_t uid);
+  void GetMatchInfo(HAL_MatchInfo* info);
   void SetMatchInfo(const HAL_MatchInfo* info);
 
+  void FreeMatchInfo(const HAL_MatchInfo* info);
+
+  int32_t RegisterNewDataCallback(HAL_NotifyCallback callback, void* param,
+                                  HAL_Bool initialNotify);
+  void CancelNewDataCallback(int32_t uid);
+  void CallNewDataCallbacks();
+
   void NotifyNewData();
 
+  void SetJoystickButton(int32_t stick, int32_t button, HAL_Bool state);
+  void SetJoystickAxis(int32_t stick, int32_t axis, double value);
+  void SetJoystickPOV(int32_t stick, int32_t pov, int32_t value);
+  void SetJoystickButtons(int32_t stick, uint32_t buttons);
+  void SetJoystickAxisCount(int32_t stick, int32_t count);
+  void SetJoystickPOVCount(int32_t stick, int32_t count);
+  void SetJoystickButtonCount(int32_t stick, int32_t count);
+  void GetJoystickCounts(int32_t stick, int32_t* axisCount,
+                         int32_t* buttonCount, int32_t* povCount);
+
+  void SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox);
+  void SetJoystickType(int32_t stick, int32_t type);
+  void SetJoystickName(int32_t stick, const char* name);
+  void SetJoystickAxisType(int32_t stick, int32_t axis, int32_t type);
+
+  void SetGameSpecificMessage(const char* message);
+  void SetEventName(const char* name);
+  void SetMatchType(HAL_MatchType type);
+  void SetMatchNumber(int32_t matchNumber);
+  void SetReplayNumber(int32_t replayNumber);
+
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetEnabledName> enabled{false};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetAutonomousName> autonomous{false};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetTestName> test{false};
@@ -71,16 +132,40 @@
   SimDataValue<double, HAL_MakeDouble, GetMatchTimeName> matchTime{0.0};
 
  private:
+  SimCallbackRegistry<HAL_JoystickAxesCallback, GetJoystickAxesName>
+      m_joystickAxesCallbacks;
+  SimCallbackRegistry<HAL_JoystickPOVsCallback, GetJoystickPOVsName>
+      m_joystickPOVsCallbacks;
+  SimCallbackRegistry<HAL_JoystickButtonsCallback, GetJoystickButtonsName>
+      m_joystickButtonsCallbacks;
+  SimCallbackRegistry<HAL_JoystickOutputsCallback, GetJoystickOutputsName>
+      m_joystickOutputsCallbacks;
+  SimCallbackRegistry<HAL_JoystickDescriptorCallback, GetJoystickDescriptorName>
+      m_joystickDescriptorCallbacks;
+  SimCallbackRegistry<HAL_MatchInfoCallback, GetMatchInfoName>
+      m_matchInfoCallbacks;
+  SimCallbackRegistry<HAL_NotifyCallback, GetNewDataName> m_newDataCallbacks;
+
+  struct JoystickOutputStore {
+    int64_t outputs = 0;
+    int32_t leftRumble = 0;
+    int32_t rightRumble = 0;
+  };
+
+  struct JoystickData {
+    HAL_JoystickAxes axes;
+    HAL_JoystickPOVs povs;
+    HAL_JoystickButtons buttons;
+    JoystickOutputStore outputs;
+    HAL_JoystickDescriptor descriptor;
+  };
+
+  static constexpr int kNumJoysticks = 6;
   wpi::spinlock m_joystickDataMutex;
+  JoystickData m_joystickData[kNumJoysticks];
+
   wpi::spinlock m_matchInfoMutex;
-
-  std::unique_ptr<HAL_JoystickAxes[]> m_joystickAxes;
-  std::unique_ptr<HAL_JoystickPOVs[]> m_joystickPOVs;
-  std::unique_ptr<HAL_JoystickButtons[]> m_joystickButtons;
-
-  std::unique_ptr<JoystickOutputStore[]> m_joystickOutputs;
-  std::unique_ptr<HAL_JoystickDescriptor[]> m_joystickDescriptor;
-  std::unique_ptr<HAL_MatchInfo> m_matchInfo;
+  HAL_MatchInfo m_matchInfo;
 };
 extern DriverStationData* SimDriverStationData;
 }  // namespace hal

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

diff --git a/hal/src/main/native/sim/mockdata/DutyCycleData.cpp b/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
index 04806e0..660522d 100644
--- a/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
+++ b/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -30,6 +30,15 @@
 }
 
 extern "C" {
+int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) {
+  for (int i = 0; i < kNumDutyCycles; ++i) {
+    if (SimDutyCycleData[i].initialized &&
+        SimDutyCycleData[i].digitalChannel == channel)
+      return i;
+  }
+  return -1;
+}
+
 void HALSIM_ResetDutyCycleData(int32_t index) {
   SimDutyCycleData[index].ResetData();
 }

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

diff --git a/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h b/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
index 2f98b07..e69f9aa 100644
--- a/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,8 @@
 #include <atomic>
 #include <limits>
 
-#include "mockdata/DutyCycleData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/DutyCycleData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class DutyCycleData {

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

diff --git a/hal/src/main/native/sim/mockdata/EncoderData.cpp b/hal/src/main/native/sim/mockdata/EncoderData.cpp
index 33bd073..0b17ac3 100644
--- a/hal/src/main/native/sim/mockdata/EncoderData.cpp
+++ b/hal/src/main/native/sim/mockdata/EncoderData.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -36,6 +36,16 @@
 }
 
 extern "C" {
+int32_t HALSIM_FindEncoderForChannel(int32_t channel) {
+  for (int i = 0; i < kNumEncoders; ++i) {
+    if (!SimEncoderData[i].initialized) continue;
+    if (SimEncoderData[i].digitalChannelA == channel ||
+        SimEncoderData[i].digitalChannelB == channel)
+      return i;
+  }
+  return -1;
+}
+
 void HALSIM_ResetEncoderData(int32_t index) {
   SimEncoderData[index].ResetData();
 }
@@ -57,7 +67,6 @@
                                SimEncoderData, LOWERNAME)
 
 DEFINE_CAPI(HAL_Bool, Initialized, initialized)
-DEFINE_CAPI(int32_t, Count, count)
 DEFINE_CAPI(double, Period, period)
 DEFINE_CAPI(HAL_Bool, Reset, reset)
 DEFINE_CAPI(double, MaxPeriod, maxPeriod)
@@ -66,6 +75,54 @@
 DEFINE_CAPI(int32_t, SamplesToAverage, samplesToAverage)
 DEFINE_CAPI(double, DistancePerPulse, distancePerPulse)
 
+int32_t HALSIM_RegisterEncoderCountCallback(int32_t index,
+                                            HAL_NotifyCallback callback,
+                                            void* param,
+                                            HAL_Bool initialNotify) {
+  return SimEncoderData[index].count.RegisterCallback(callback, param,
+                                                      initialNotify);
+}
+
+void HALSIM_CancelEncoderCountCallback(int32_t index, int32_t uid) {
+  SimEncoderData[index].count.CancelCallback(uid);
+}
+
+int32_t HALSIM_GetEncoderCount(int32_t index) {
+  return SimEncoderData[index].count;
+}
+
+void HALSIM_SetEncoderCount(int32_t index, int32_t count) {
+  SimEncoderData[index].count = count;
+  SimEncoderData[index].reset = false;
+}
+
+void HALSIM_SetEncoderDistance(int32_t index, double distance) {
+  auto& simData = SimEncoderData[index];
+  simData.count = distance / simData.distancePerPulse;
+  simData.reset = false;
+}
+
+double HALSIM_GetEncoderDistance(int32_t index) {
+  auto& simData = SimEncoderData[index];
+  return simData.count * simData.distancePerPulse;
+}
+
+void HALSIM_SetEncoderRate(int32_t index, double rate) {
+  auto& simData = SimEncoderData[index];
+  if (rate == 0) {
+    simData.period = std::numeric_limits<double>::infinity();
+    return;
+  }
+
+  simData.period = simData.distancePerPulse / rate;
+}
+
+double HALSIM_GetEncoderRate(int32_t index) {
+  auto& simData = SimEncoderData[index];
+
+  return simData.distancePerPulse / simData.period;
+}
+
 #define REGISTER(NAME) \
   SimEncoderData[index].NAME.RegisterCallback(callback, param, initialNotify)
 

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

diff --git a/hal/src/main/native/sim/mockdata/EncoderDataInternal.h b/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
index 389289c..3848f1d 100644
--- a/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,8 @@
 #include <atomic>
 #include <limits>
 
-#include "mockdata/EncoderData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/EncoderData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class EncoderData {

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

diff --git a/hal/src/main/native/sim/mockdata/I2CData.cpp b/hal/src/main/native/sim/mockdata/I2CData.cpp
index b228c3b..713064e 100644
--- a/hal/src/main/native/sim/mockdata/I2CData.cpp
+++ b/hal/src/main/native/sim/mockdata/I2CData.cpp
@@ -1,12 +1,10 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <iostream>
-
 #include "../PortsInternal.h"
 #include "I2CDataInternal.h"
 

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

diff --git a/hal/src/main/native/sim/mockdata/I2CDataInternal.h b/hal/src/main/native/sim/mockdata/I2CDataInternal.h
index c799bb7..4822222 100644
--- a/hal/src/main/native/sim/mockdata/I2CDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/I2CDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,9 +7,9 @@
 
 #pragma once
 
-#include "mockdata/I2CData.h"
-#include "mockdata/SimCallbackRegistry.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/I2CData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class I2CData {

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

diff --git a/hal/src/main/native/sim/mockdata/PCMDataInternal.h b/hal/src/main/native/sim/mockdata/PCMDataInternal.h
index 0c7b99b..0d3752e 100644
--- a/hal/src/main/native/sim/mockdata/PCMDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/PCMDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,8 +8,8 @@
 #pragma once
 
 #include "../PortsInternal.h"
-#include "mockdata/PCMData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/PCMData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class PCMData {

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

diff --git a/hal/src/main/native/sim/mockdata/PDPDataInternal.h b/hal/src/main/native/sim/mockdata/PDPDataInternal.h
index 8d45416..3392eaa 100644
--- a/hal/src/main/native/sim/mockdata/PDPDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/PDPDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,8 +8,8 @@
 #pragma once
 
 #include "../PortsInternal.h"
-#include "mockdata/PDPData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/PDPData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class PDPData {

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

diff --git a/hal/src/main/native/sim/mockdata/PWMDataInternal.h b/hal/src/main/native/sim/mockdata/PWMDataInternal.h
index 248b7b3..028e25a 100644
--- a/hal/src/main/native/sim/mockdata/PWMDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/PWMDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/PWMData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/PWMData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class PWMData {

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

diff --git a/hal/src/main/native/sim/mockdata/RelayDataInternal.h b/hal/src/main/native/sim/mockdata/RelayDataInternal.h
index cb38388..d62ea2f 100644
--- a/hal/src/main/native/sim/mockdata/RelayDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/RelayDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/RelayData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/RelayData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class RelayData {

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

diff --git a/hal/src/main/native/sim/mockdata/RoboRioData.cpp b/hal/src/main/native/sim/mockdata/RoboRioData.cpp
index 5cff1d9..76406f5 100644
--- a/hal/src/main/native/sim/mockdata/RoboRioData.cpp
+++ b/hal/src/main/native/sim/mockdata/RoboRioData.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,8 +13,8 @@
 namespace hal {
 namespace init {
 void InitializeRoboRioData() {
-  static RoboRioData srrd[1];
-  ::hal::SimRoboRioData = srrd;
+  static RoboRioData srrd;
+  ::hal::SimRoboRioData = &srrd;
 }
 }  // namespace init
 }  // namespace hal
@@ -22,30 +22,28 @@
 RoboRioData* hal::SimRoboRioData;
 void RoboRioData::ResetData() {
   fpgaButton.Reset(false);
-  vInVoltage.Reset(0.0);
+  vInVoltage.Reset(12.0);
   vInCurrent.Reset(0.0);
   userVoltage6V.Reset(6.0);
   userCurrent6V.Reset(0.0);
-  userActive6V.Reset(false);
+  userActive6V.Reset(true);
   userVoltage5V.Reset(5.0);
   userCurrent5V.Reset(0.0);
-  userActive5V.Reset(false);
+  userActive5V.Reset(true);
   userVoltage3V3.Reset(3.3);
   userCurrent3V3.Reset(0.0);
-  userActive3V3.Reset(false);
+  userActive3V3.Reset(true);
   userFaults6V.Reset(0);
   userFaults5V.Reset(0);
   userFaults3V3.Reset(0);
 }
 
 extern "C" {
-void HALSIM_ResetRoboRioData(int32_t index) {
-  SimRoboRioData[index].ResetData();
-}
+void HALSIM_ResetRoboRioData(void) { SimRoboRioData->ResetData(); }
 
-#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                  \
-  HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, RoboRio##CAPINAME, \
-                               SimRoboRioData, LOWERNAME)
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                          \
+  HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, RoboRio##CAPINAME, \
+                                       SimRoboRioData, LOWERNAME)
 
 DEFINE_CAPI(HAL_Bool, FPGAButton, fpgaButton)
 DEFINE_CAPI(double, VInVoltage, vInVoltage)
@@ -64,10 +62,9 @@
 DEFINE_CAPI(int32_t, UserFaults3V3, userFaults3V3)
 
 #define REGISTER(NAME) \
-  SimRoboRioData[index].NAME.RegisterCallback(callback, param, initialNotify)
+  SimRoboRioData->NAME.RegisterCallback(callback, param, initialNotify)
 
-void HALSIM_RegisterRoboRioAllCallbacks(int32_t index,
-                                        HAL_NotifyCallback callback,
+void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
                                         void* param, HAL_Bool initialNotify) {
   REGISTER(fpgaButton);
   REGISTER(vInVoltage);

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

diff --git a/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h b/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
index cc74fa2..2fb3456 100644
--- a/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/RoboRioData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/RoboRioData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class RoboRioData {
@@ -30,22 +30,22 @@
 
  public:
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFPGAButtonName> fpgaButton{false};
-  SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{0.0};
+  SimDataValue<double, HAL_MakeDouble, GetVInVoltageName> vInVoltage{12.0};
   SimDataValue<double, HAL_MakeDouble, GetVInCurrentName> vInCurrent{0.0};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage6VName> userVoltage6V{6.0};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent6VName> userCurrent6V{0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive6VName> userActive6V{
-      false};
+      true};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage5VName> userVoltage5V{5.0};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent5VName> userCurrent5V{0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive5VName> userActive5V{
-      false};
+      true};
   SimDataValue<double, HAL_MakeDouble, GetUserVoltage3V3Name> userVoltage3V3{
       3.3};
   SimDataValue<double, HAL_MakeDouble, GetUserCurrent3V3Name> userCurrent3V3{
       0.0};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetUserActive3V3Name> userActive3V3{
-      false};
+      true};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults6VName> userFaults6V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults5VName> userFaults5V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults3V3Name> userFaults3V3{0};

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

diff --git a/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h b/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
index 661e01b..59c6685 100644
--- a/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,8 @@
 
 #pragma once
 
-#include "mockdata/SPIAccelerometerData.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/SPIAccelerometerData.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 class SPIAccelerometerData {

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

diff --git a/hal/src/main/native/sim/mockdata/SPIData.cpp b/hal/src/main/native/sim/mockdata/SPIData.cpp
index 3afc606..106ab7f 100644
--- a/hal/src/main/native/sim/mockdata/SPIData.cpp
+++ b/hal/src/main/native/sim/mockdata/SPIData.cpp
@@ -1,12 +1,10 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <iostream>
-
 #include "../PortsInternal.h"
 #include "SPIDataInternal.h"
 

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

diff --git a/hal/src/main/native/sim/mockdata/SPIDataInternal.h b/hal/src/main/native/sim/mockdata/SPIDataInternal.h
index 44868d2..ff0a6c9 100644
--- a/hal/src/main/native/sim/mockdata/SPIDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/SPIDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,9 +7,9 @@
 
 #pragma once
 
-#include "mockdata/SPIData.h"
-#include "mockdata/SimCallbackRegistry.h"
-#include "mockdata/SimDataValue.h"
+#include "hal/simulation/SPIData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
+#include "hal/simulation/SimDataValue.h"
 
 namespace hal {
 

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

diff --git a/hal/src/main/native/sim/mockdata/SimDeviceData.cpp b/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
index a703257..6c0a7f2 100644
--- a/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
+++ b/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
@@ -1,11 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include "mockdata/SimDeviceData.h"  // NOLINT(build/include_order)
+#include "hal/simulation/SimDeviceData.h"  // NOLINT(build/include_order)
 
 #include <algorithm>
 
@@ -49,12 +49,43 @@
   return deviceImpl->values[handle].get();
 }
 
+void SimDeviceData::SetDeviceEnabled(const char* prefix, bool enabled) {
+  std::scoped_lock lock(m_mutex);
+  auto it =
+      std::find_if(m_prefixEnabled.begin(), m_prefixEnabled.end(),
+                   [=](const auto& elem) { return elem.first == prefix; });
+  if (it != m_prefixEnabled.end()) {
+    it->second = enabled;
+    return;
+  }
+  m_prefixEnabled.emplace_back(prefix, enabled);
+  // keep it sorted by name
+  // string comparison sorts shorter before longer, so reverse the sort
+  std::sort(m_prefixEnabled.begin(), m_prefixEnabled.end(),
+            [](const auto& l, const auto& r) { return l.first >= r.first; });
+}
+
+bool SimDeviceData::IsDeviceEnabled(const char* name) {
+  std::scoped_lock lock(m_mutex);
+  for (const auto& elem : m_prefixEnabled) {
+    if (wpi::StringRef{name}.startswith(elem.first)) return elem.second;
+  }
+  return true;
+}
+
 HAL_SimDeviceHandle SimDeviceData::CreateDevice(const char* name) {
   std::scoped_lock lock(m_mutex);
 
+  // don't create if disabled
+  for (const auto& elem : m_prefixEnabled) {
+    if (wpi::StringRef{name}.startswith(elem.first)) {
+      if (elem.second) break;  // enabled
+      return 0;                // disabled
+    }
+  }
+
   // check for duplicates and don't overwrite them
-  auto it = m_deviceMap.find(name);
-  if (it != m_deviceMap.end()) return 0;
+  if (m_deviceMap.count(name) > 0) return 0;
 
   // don't allow more than 4096 devices (limit driven by 12-bit allocation in
   // value changed callback uid)
@@ -326,12 +357,21 @@
   std::scoped_lock lock(m_mutex);
   m_devices.clear();
   m_deviceMap.clear();
+  m_prefixEnabled.clear();
   m_deviceCreated.Reset();
   m_deviceFreed.Reset();
 }
 
 extern "C" {
 
+void HALSIM_SetSimDeviceEnabled(const char* prefix, HAL_Bool enabled) {
+  SimSimDeviceData->SetDeviceEnabled(prefix, enabled);
+}
+
+HAL_Bool HALSIM_IsSimDeviceEnabled(const char* name) {
+  return SimSimDeviceData->IsDeviceEnabled(name);
+}
+
 int32_t HALSIM_RegisterSimDeviceCreatedCallback(
     const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
     HAL_Bool initialNotify) {

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

diff --git a/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h b/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
index 2582eca..63c2288 100644
--- a/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
+++ b/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,8 +19,8 @@
 #include <wpi/spinlock.h>
 
 #include "hal/Value.h"
-#include "mockdata/SimCallbackRegistry.h"
-#include "mockdata/SimDeviceData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
+#include "hal/simulation/SimDeviceData.h"
 
 namespace hal {
 
@@ -150,6 +150,7 @@
 
   wpi::UidVector<std::shared_ptr<Device>, 4> m_devices;
   wpi::StringMap<std::weak_ptr<Device>> m_deviceMap;
+  std::vector<std::pair<std::string, bool>> m_prefixEnabled;
 
   wpi::recursive_spinlock m_mutex;
 
@@ -161,6 +162,9 @@
   Value* LookupValue(HAL_SimValueHandle handle);
 
  public:
+  void SetDeviceEnabled(const char* prefix, bool enabled);
+  bool IsDeviceEnabled(const char* name);
+
   HAL_SimDeviceHandle CreateDevice(const char* name);
   void FreeDevice(HAL_SimDeviceHandle handle);
   HAL_SimValueHandle CreateValue(HAL_SimDeviceHandle device, const char* name,

hal/src/test/java/edu/wpi/first/hal/sim/AccelerometerSimTest.java

diff --git a/hal/src/test/java/edu/wpi/first/hal/sim/AccelerometerSimTest.java b/hal/src/test/java/edu/wpi/first/hal/sim/AccelerometerSimTest.java
deleted file mode 100644
index 383165c..0000000
--- a/hal/src/test/java/edu/wpi/first/hal/sim/AccelerometerSimTest.java
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.hal.AccelerometerJNI;
-import edu.wpi.first.hal.HAL;
-
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class AccelerometerSimTest {
-  static class TriggeredStore {
-    public boolean m_wasTriggered;
-    public boolean m_setValue = true;
-  }
-
-  @Test
-  void testCallbacks() {
-    HAL.initialize(500, 0);
-    AccelerometerSim sim = new AccelerometerSim();
-    sim.resetData();
-
-    TriggeredStore store = new TriggeredStore();
-
-    try (CallbackStore cb = sim.registerActiveCallback((s, v) -> {
-      store.m_wasTriggered = true;
-      store.m_setValue = v.getBoolean();
-    }, false)) {
-      assertFalse(store.m_wasTriggered);
-      AccelerometerJNI.setAccelerometerActive(true);
-      assertTrue(store.m_wasTriggered);
-      assertTrue(store.m_setValue);
-    }
-  }
-}

hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java

diff --git a/hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java b/hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java
deleted file mode 100644
index 29452a0..0000000
--- a/hal/src/test/java/edu/wpi/first/hal/sim/SimDeviceSimTest.java
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.sim;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.hal.SimBoolean;
-import edu.wpi.first.hal.SimDevice;
-
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class SimDeviceSimTest {
-  @Test
-  void testBasic() {
-    SimDevice dev = SimDevice.create("test");
-    SimBoolean devBool = dev.createBoolean("bool", false, false);
-
-    SimDeviceSim sim = new SimDeviceSim("test");
-    SimBoolean simBool = sim.getBoolean("bool");
-
-    assertFalse(simBool.get());
-    simBool.set(true);
-    assertTrue(devBool.get());
-  }
-}

hal/src/test/native/cpp/can/CANTest.cpp

diff --git a/hal/src/test/native/cpp/can/CANTest.cpp b/hal/src/test/native/cpp/can/CANTest.cpp
index 6f5549c..427bb54 100644
--- a/hal/src/test/native/cpp/can/CANTest.cpp
+++ b/hal/src/test/native/cpp/can/CANTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,7 @@
 #include "gtest/gtest.h"
 #include "hal/CANAPI.h"
 #include "hal/HAL.h"
-#include "mockdata/CanData.h"
+#include "hal/simulation/CanData.h"
 
 namespace hal {
 struct CANTestStore {

hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp b/hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp
index 791be79..c102480 100644
--- a/hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/AnalogInput.h"
 #include "hal/HAL.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/AnalogInData.h"
+#include "hal/simulation/AnalogInData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp b/hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp
index 8f6d6f0..d6c9f70 100644
--- a/hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/AnalogOutput.h"
 #include "hal/HAL.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/AnalogOutData.h"
+#include "hal/simulation/AnalogOutData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/DIODataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/DIODataTests.cpp b/hal/src/test/native/cpp/mockdata/DIODataTests.cpp
index 19fe994..248f841 100644
--- a/hal/src/test/native/cpp/mockdata/DIODataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/DIODataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/DIO.h"
 #include "hal/HAL.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/DIOData.h"
+#include "hal/simulation/DIOData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp b/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
index 5505c06..5cb28c4 100644
--- a/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,7 @@
 #include "gtest/gtest.h"
 #include "hal/HAL.h"
 #include "hal/Solenoid.h"
-#include "mockdata/DriverStationData.h"
+#include "hal/simulation/DriverStationData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/I2CDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/I2CDataTests.cpp b/hal/src/test/native/cpp/mockdata/I2CDataTests.cpp
index 3a8e01c..b78564b 100644
--- a/hal/src/test/native/cpp/mockdata/I2CDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/I2CDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/HAL.h"
 #include "hal/I2C.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/I2CData.h"
+#include "hal/simulation/I2CData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/PCMDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/PCMDataTests.cpp b/hal/src/test/native/cpp/mockdata/PCMDataTests.cpp
index 50a8ae3..5fc19aa 100644
--- a/hal/src/test/native/cpp/mockdata/PCMDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/PCMDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/HAL.h"
 #include "hal/Solenoid.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/PCMData.h"
+#include "hal/simulation/PCMData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/PDPDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp b/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp
index a46454f..f980c7c 100644
--- a/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/HAL.h"
 #include "hal/PDP.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/PDPData.h"
+#include "hal/simulation/PDPData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/PWMDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/PWMDataTests.cpp b/hal/src/test/native/cpp/mockdata/PWMDataTests.cpp
index 447a510..daca364 100644
--- a/hal/src/test/native/cpp/mockdata/PWMDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/PWMDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/HAL.h"
 #include "hal/PWM.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/PWMData.h"
+#include "hal/simulation/PWMData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/RelayDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/RelayDataTests.cpp b/hal/src/test/native/cpp/mockdata/RelayDataTests.cpp
index 408657a..edc0fb4 100644
--- a/hal/src/test/native/cpp/mockdata/RelayDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/RelayDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/HAL.h"
 #include "hal/Relay.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/RelayData.h"
+#include "hal/simulation/RelayData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/SPIDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/SPIDataTests.cpp b/hal/src/test/native/cpp/mockdata/SPIDataTests.cpp
index 64c8555..423db6d 100644
--- a/hal/src/test/native/cpp/mockdata/SPIDataTests.cpp
+++ b/hal/src/test/native/cpp/mockdata/SPIDataTests.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 #include "hal/HAL.h"
 #include "hal/SPI.h"
 #include "hal/handles/HandlesInternal.h"
-#include "mockdata/SPIData.h"
+#include "hal/simulation/SPIData.h"
 
 namespace hal {
 

hal/src/test/native/cpp/mockdata/SimDeviceDataTests.cpp

diff --git a/hal/src/test/native/cpp/mockdata/SimDeviceDataTests.cpp b/hal/src/test/native/cpp/mockdata/SimDeviceDataTests.cpp
new file mode 100644
index 0000000..8f65f5c
--- /dev/null
+++ b/hal/src/test/native/cpp/mockdata/SimDeviceDataTests.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "gtest/gtest.h"
+#include "hal/SimDevice.h"
+#include "hal/simulation/SimDeviceData.h"
+
+namespace hal {
+
+TEST(SimDeviceSimTests, TestEnabled) {
+  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("foo"));
+  HALSIM_SetSimDeviceEnabled("f", false);
+  HALSIM_SetSimDeviceEnabled("foob", true);
+  ASSERT_FALSE(HALSIM_IsSimDeviceEnabled("foo"));
+  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("foobar"));
+  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("bar"));
+
+  ASSERT_EQ(HAL_CreateSimDevice("foo"), 0);
+}
+
+}  // namespace hal

hal/src/test/native/cpp/sim/AccelerometerSimTest.cpp

diff --git a/hal/src/test/native/cpp/sim/AccelerometerSimTest.cpp b/hal/src/test/native/cpp/sim/AccelerometerSimTest.cpp
deleted file mode 100644
index 54be6e3..0000000
--- a/hal/src/test/native/cpp/sim/AccelerometerSimTest.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/Accelerometer.h"
-#include "hal/HAL.h"
-#include "simulation/AccelerometerSim.h"
-
-using namespace frc::sim;
-
-namespace hal {
-
-TEST(AcclerometerSimTests, TestActiveCallback) {
-  HAL_Initialize(500, 0);
-
-  AccelerometerSim sim{0};
-
-  sim.ResetData();
-
-  bool wasTriggered = false;
-  bool lastValue = false;
-
-  auto cb = sim.RegisterActiveCallback(
-      [&](wpi::StringRef name, const HAL_Value* value) {
-        wasTriggered = true;
-        lastValue = value->data.v_boolean;
-      },
-      false);
-
-  EXPECT_FALSE(wasTriggered);
-
-  HAL_SetAccelerometerActive(true);
-
-  EXPECT_TRUE(wasTriggered);
-  EXPECT_TRUE(lastValue);
-}
-
-}  // namespace hal

hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp

diff --git a/hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp b/hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp
deleted file mode 100644
index ba0646d..0000000
--- a/hal/src/test/native/cpp/sim/SimDeviceSimTest.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <wpi/StringRef.h>
-
-#include "gtest/gtest.h"
-#include "hal/SimDevice.h"
-#include "simulation/SimDeviceSim.h"
-
-using namespace frc::sim;
-
-namespace hal {
-
-TEST(SimDeviceSimTests, TestBasic) {
-  SimDevice dev{"test"};
-  SimBoolean devBool = dev.CreateBoolean("bool", false, false);
-
-  SimDeviceSim sim{"test"};
-  SimBoolean simBool = sim.GetBoolean("bool");
-  EXPECT_FALSE(simBool.Get());
-  simBool.Set(true);
-  EXPECT_TRUE(devBool.Get());
-}
-
-TEST(SimDeviceSimTests, TestEnumerateDevices) {
-  SimDevice dev{"test"};
-
-  bool foundit = false;
-  SimDeviceSim::EnumerateDevices(
-      "te", [&](const char* name, HAL_SimDeviceHandle handle) {
-        if (wpi::StringRef(name) == "test") foundit = true;
-      });
-  EXPECT_TRUE(foundit);
-}
-
-}  // namespace hal

hal/src/test/native/cpp/sim/SimInitializationTest.cpp

diff --git a/hal/src/test/native/cpp/sim/SimInitializationTest.cpp b/hal/src/test/native/cpp/sim/SimInitializationTest.cpp
deleted file mode 100644
index fdd34cd..0000000
--- a/hal/src/test/native/cpp/sim/SimInitializationTest.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "simulation/AccelerometerSim.h"
-#include "simulation/AnalogGyroSim.h"
-#include "simulation/AnalogInSim.h"
-#include "simulation/AnalogOutSim.h"
-#include "simulation/AnalogTriggerSim.h"
-#include "simulation/DIOSim.h"
-#include "simulation/DigitalPWMSim.h"
-#include "simulation/DriverStationSim.h"
-#include "simulation/EncoderSim.h"
-#include "simulation/PCMSim.h"
-#include "simulation/PDPSim.h"
-#include "simulation/PWMSim.h"
-#include "simulation/RelaySim.h"
-#include "simulation/RoboRioSim.h"
-#include "simulation/SPIAccelerometerSim.h"
-
-using namespace frc::sim;
-
-namespace hal {
-
-TEST(SimInitializationTests, TestAllInitialize) {
-  HAL_Initialize(500, 0);
-  AccelerometerSim acsim{0};
-  AnalogGyroSim agsim{0};
-  AnalogInSim aisim{0};
-  AnalogOutSim aosim{0};
-  AnalogTriggerSim atsim{0};
-  DigitalPWMSim dpsim{0};
-  DIOSim diosim{0};
-  DriverStationSim dssim;
-  (void)dssim;
-  EncoderSim esim{0};
-  PCMSim pcmsim{0};
-  PDPSim pdpsim{0};
-  PWMSim pwmsim{0};
-  RelaySim rsim{0};
-  RoboRioSim rrsim{0};
-  SPIAccelerometerSim sasim{0};
-}
-}  // namespace hal

imgui/CMakeLists.txt

diff --git a/imgui/CMakeLists.txt b/imgui/CMakeLists.txt
index 664c045..82b1592 100644
--- a/imgui/CMakeLists.txt
+++ b/imgui/CMakeLists.txt
@@ -32,6 +32,11 @@
 set_source_files_properties(${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp
     PROPERTIES OBJECT_DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/ProggyDotted.inc)
 
+# stb_image
+file(GENERATE OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp
+    CONTENT "#define STBI_WINDOWS_UTF8\n#define STB_IMAGE_IMPLEMENTATION\n#include \"stb_image.h\"\n"
+)
+
 # Add imgui directly to our build.
 set(SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS})
 set(BUILD_SHARED_LIBS OFF)
@@ -46,8 +51,23 @@
 
 set(imgui_srcdir ${CMAKE_CURRENT_BINARY_DIR}/imgui-src)
 file(GLOB imgui_sources ${imgui_srcdir}/*.cpp)
-add_library(imgui STATIC ${imgui_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp)
+set(implot_srcdir ${CMAKE_CURRENT_BINARY_DIR}/implot-src)
+file(GLOB implot_sources ${implot_srcdir}/*.cpp)
+add_library(imgui STATIC ${imgui_sources} ${implot_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp)
+target_compile_definitions(imgui PUBLIC IMGUI_IMPL_OPENGL_LOADER_GL3W)
+if (MSVC)
+    target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_directx11.cpp)
+else()
+    if (APPLE)
+        target_compile_options(imgui PRIVATE -fobjc-arc)
+        set_target_properties(imgui PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore")
+        target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_metal.mm)
+    else()
+        #target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp)
+    endif()
+endif()
 target_link_libraries(imgui PUBLIC gl3w glfw)
-target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
+target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${implot_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/stb-src>")
 
 set_property(TARGET imgui PROPERTY POSITION_INDEPENDENT_CODE ON)
+target_compile_features(imgui PUBLIC cxx_std_17)

imgui/CMakeLists.txt.in

diff --git a/imgui/CMakeLists.txt.in b/imgui/CMakeLists.txt.in
index a5c6091..998837d 100644
--- a/imgui/CMakeLists.txt.in
+++ b/imgui/CMakeLists.txt.in
@@ -5,7 +5,7 @@
 include(ExternalProject)
 ExternalProject_Add(glfw3
   GIT_REPOSITORY    https://github.com/glfw/glfw.git
-  GIT_TAG           7105ff2dfd004a46bd732c1d0c9f461bae6d51b3
+  GIT_TAG           63af05c41961c238c2f891e2c923e1b89c1271b6
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/glfw-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/glfw-build"
   CONFIGURE_COMMAND ""
@@ -23,7 +23,7 @@
 )
 ExternalProject_Add(imgui
   GIT_REPOSITORY    https://github.com/ocornut/imgui.git
-  GIT_TAG           v1.72b
+  GIT_TAG           v1.76
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/imgui-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/imgui-build"
   CONFIGURE_COMMAND ""
@@ -31,6 +31,16 @@
   INSTALL_COMMAND   ""
   TEST_COMMAND      ""
 )
+ExternalProject_Add(implot
+  GIT_REPOSITORY    https://github.com/epezent/implot.git
+  GIT_TAG           90693cca1bd0ca5f0d49bc9cb8187d56b0b8f289
+  SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/implot-src"
+  BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/implot-build"
+  CONFIGURE_COMMAND ""
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+)
 ExternalProject_Add(proggyfonts
   GIT_REPOSITORY    https://github.com/bluescan/proggyfonts.git
   GIT_TAG           v1.1.5
@@ -41,3 +51,13 @@
   INSTALL_COMMAND   ""
   TEST_COMMAND      ""
 )
+ExternalProject_Add(stb
+  GIT_REPOSITORY    https://github.com/nothings/stb.git
+  GIT_TAG           f54acd4e13430c5122cab4ca657705c84aa61b08
+  SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/stb-src"
+  BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/stb-build"
+  CONFIGURE_COMMAND ""
+  BUILD_COMMAND     ""
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+)

myRobot/build.gradle

diff --git a/myRobot/build.gradle b/myRobot/build.gradle
index d2f1194..4d6bb26 100644
--- a/myRobot/build.gradle
+++ b/myRobot/build.gradle
@@ -1,5 +1,6 @@
 import jaci.gradle.toolchains.*
 import jaci.gradle.nativedeps.*
+import org.gradle.internal.os.OperatingSystem
 
 plugins {
     id 'java'
@@ -13,6 +14,12 @@
 
 apply from: '../shared/config.gradle'
 
+application {
+    if (OperatingSystem.current().isMacOsX()) {
+        applicationDefaultJvmArgs = ['-XstartOnFirstThread']
+    }
+}
+
 ext {
     sharedCvConfigs = [myRobotCpp: []]
     staticCvConfigs = [myRobotCppStatic: []]
@@ -109,7 +116,7 @@
     }
 }
 
-mainClassName = 'Main'
+mainClassName = 'frc.robot.Main'
 
 apply plugin: 'com.github.johnrengelman.shadow'
 
@@ -119,6 +126,7 @@
 
 dependencies {
     implementation project(':wpilibj')
+    implementation project(':wpimath')
     implementation project(':hal')
     implementation project(':wpiutil')
     implementation project(':ntcore')
@@ -128,6 +136,10 @@
     implementation project(':wpilibNewCommands')
 }
 
+def simProjects = [
+    'halsim_gui'
+]
+
 model {
     components {
         myRobotCpp(NativeExecutableSpec) {
@@ -145,20 +157,28 @@
                 }
             }
             binaries.all { binary ->
-                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
-                    lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
-                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
-                    lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
-                    lib project: ':cscore', library: 'cscore', linkage: 'shared'
-                    lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
-                    lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
-                    project(':hal').addHalDependency(binary, 'shared')
-                    project(':hal').addHalJniDependency(binary)
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                    lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
-                    if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
+                lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
+                lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+                lib project: ':cscore', library: 'cscore', linkage: 'shared'
+                lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+                lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
+                project(':hal').addHalDependency(binary, 'shared')
+                project(':hal').addHalJniDependency(binary)
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+                if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                } else {
+                    def systemArch = getCurrentArch()
+                    if (binary.targetPlatform.name == systemArch) {
+                        simProjects.each {
+                            lib project: ":simulation:$it", library: it, linkage: 'shared'
+                        }
                     }
+                }
             }
         }
         myRobotCppStatic(NativeExecutableSpec) {
@@ -177,17 +197,18 @@
                 }
             }
             binaries.all { binary ->
-                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'static'
-                    lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static'
-                    lib project: ':wpilibc', library: 'wpilibc', linkage: 'static'
-                    lib project: ':ntcore', library: 'ntcore', linkage: 'static'
-                    lib project: ':cscore', library: 'cscore', linkage: 'static'
-                    project(':hal').addHalDependency(binary, 'static')
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
-                    lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
-                    if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
-                    }
+                lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'static'
+                lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static'
+                lib project: ':wpilibc', library: 'wpilibc', linkage: 'static'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':ntcore', library: 'ntcore', linkage: 'static'
+                lib project: ':cscore', library: 'cscore', linkage: 'static'
+                project(':hal').addHalDependency(binary, 'static')
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+                lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
+                if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                }
             }
         }
     }
@@ -198,6 +219,7 @@
             description = "Run the myRobotCpp executable"
             def found = false
             def systemArch = getCurrentArch()
+            def runTask = it
             c.each {
                 if (it in NativeExecutableSpec && it.name == "myRobotCpp") {
                     it.binaries.each {
@@ -210,6 +232,32 @@
                                 run.dependsOn it.tasks.install
                                 run.systemProperty 'java.library.path', filePath
                                 run.environment 'LD_LIBRARY_PATH', filePath
+
+                                def installTask = it.tasks.install
+
+                                def doFirstTask = {
+                                    def extensions = '';
+                                    installTask.installDirectory.get().getAsFile().eachFileRecurse {
+                                        def name = it.name
+                                        if (!(name.endsWith('.dll') || name.endsWith('.so') || name.endsWith('.dylib'))) {
+                                            return
+                                        }
+                                        def file = it
+                                        simProjects.each {
+                                            if (name.startsWith(it) || name.startsWith("lib$it".toString())) {
+                                                extensions += file.absolutePath + File.pathSeparator
+                                            }
+                                        }
+                                    }
+                                    if (extensions != '') {
+                                        run.environment 'HALSIM_EXTENSIONS', extensions
+                                        runTask.environment 'HALSIM_EXTENSIONS', extensions
+                                    }
+                                }
+
+                                runTask.doFirst doFirstTask
+                                run.doFirst doFirstTask
+
                                 run.workingDir filePath
 
                                 found = true

myRobot/src/main/java/Main.java

diff --git a/myRobot/src/main/java/Main.java b/myRobot/src/main/java/Main.java
deleted file mode 100644
index aee472a..0000000
--- a/myRobot/src/main/java/Main.java
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-public final class Main {
-  private Main() {
-  }
-
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(MyRobot::new);
-  }
-}

myRobot/src/main/java/MyRobot.java

diff --git a/myRobot/src/main/java/MyRobot.java b/myRobot/src/main/java/MyRobot.java
deleted file mode 100644
index fc69d42..0000000
--- a/myRobot/src/main/java/MyRobot.java
+++ /dev/null
@@ -1,54 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-import edu.wpi.first.wpilibj.TimedRobot;
-
-@SuppressWarnings("all")
-public class MyRobot extends TimedRobot {
-  /**
-   * This function is run when the robot is first started up and should be
-   * used for any initialization code.
-   */
-  @Override
-  public void robotInit() {}
-
-  /**
-   * This function is run once each time the robot enters autonomous mode
-   */
-  @Override
-  public void autonomousInit() {}
-
-  /**
-   * This function is called periodically during autonomous
-   */
-  @Override
-  public void autonomousPeriodic() {}
-
-  /**
-   * This function is called once each time the robot enters tele-operated mode
-   */
-  @Override
-  public void teleopInit() {}
-
-  /**
-   * This function is called periodically during operator control
-   */
-  @Override
-  public void teleopPeriodic() {}
-
-  /**
-   * This function is called periodically during test mode
-   */
-  @Override
-  public void testPeriodic() {}
-
-  /**
-   * This function is called periodically during all modes
-   */
-  @Override
-  public void robotPeriodic() {}
-}

myRobot/src/main/java/frc/robot/Main.java

diff --git a/myRobot/src/main/java/frc/robot/Main.java b/myRobot/src/main/java/frc/robot/Main.java
new file mode 100644
index 0000000..070dd31
--- /dev/null
+++ b/myRobot/src/main/java/frc/robot/Main.java
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(MyRobot::new);
+  }
+}

myRobot/src/main/java/frc/robot/MyRobot.java

diff --git a/myRobot/src/main/java/frc/robot/MyRobot.java b/myRobot/src/main/java/frc/robot/MyRobot.java
new file mode 100644
index 0000000..3166944
--- /dev/null
+++ b/myRobot/src/main/java/frc/robot/MyRobot.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+
+@SuppressWarnings("all")
+public class MyRobot extends TimedRobot {
+  /**
+   * This function is run when the robot is first started up and should be
+   * used for any initialization code.
+   */
+  @Override
+  public void robotInit() {}
+
+  /**
+   * This function is run once each time the robot enters autonomous mode
+   */
+  @Override
+  public void autonomousInit() {}
+
+  /**
+   * This function is called periodically during autonomous
+   */
+  @Override
+  public void autonomousPeriodic() {}
+
+  /**
+   * This function is called once each time the robot enters tele-operated mode
+   */
+  @Override
+  public void teleopInit() {}
+
+  /**
+   * This function is called periodically during operator control
+   */
+  @Override
+  public void teleopPeriodic() {}
+
+  /**
+   * This function is called periodically during test mode
+   */
+  @Override
+  public void testPeriodic() {}
+
+  /**
+   * This function is called periodically during all modes
+   */
+  @Override
+  public void robotPeriodic() {}
+}

ntcore/.styleguide

diff --git a/ntcore/.styleguide b/ntcore/.styleguide
index 4c00fa9..67befdb 100644
--- a/ntcore/.styleguide
+++ b/ntcore/.styleguide
@@ -5,6 +5,7 @@
 cppHeaderFileInclude {
   (?<!_c)\.h$
   \.inc$
+  \.inl$
 }
 
 cppSrcFileInclude {

ntcore/CMakeLists.txt

diff --git a/ntcore/CMakeLists.txt b/ntcore/CMakeLists.txt
index 1852702..e095fa9 100644
--- a/ntcore/CMakeLists.txt
+++ b/ntcore/CMakeLists.txt
@@ -20,7 +20,7 @@
 install(TARGETS ntcore EXPORT ntcore DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/ntcore")
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
     set (ntcore_config_dir ${wpilib_dest})
 else()
     set (ntcore_config_dir share/ntcore)
@@ -31,7 +31,7 @@
 install(EXPORT ntcore DESTINATION ${ntcore_config_dir})
 
 # Java bindings
-if (NOT WITHOUT_JAVA)
+if (WITH_JAVA)
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)

ntcore/manualTests/native/client.cpp

diff --git a/ntcore/manualTests/native/client.cpp b/ntcore/manualTests/native/client.cpp
index 3bcb7c0..bb16a9c 100644
--- a/ntcore/manualTests/native/client.cpp
+++ b/ntcore/manualTests/native/client.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,12 +14,13 @@
 
 int main() {
   auto inst = nt::GetDefaultInstance();
-  nt::AddLogger(inst,
-                [](const nt::LogMessage& msg) {
-                  std::fputs(msg.message.c_str(), stderr);
-                  std::fputc('\n', stderr);
-                },
-                0, UINT_MAX);
+  nt::AddLogger(
+      inst,
+      [](const nt::LogMessage& msg) {
+        std::fputs(msg.message.c_str(), stderr);
+        std::fputc('\n', stderr);
+      },
+      0, UINT_MAX);
   nt::StartClient(inst, "127.0.0.1", 10000);
   std::this_thread::sleep_for(std::chrono::seconds(2));
 

ntcore/manualTests/native/rpc_local.cpp

diff --git a/ntcore/manualTests/native/rpc_local.cpp b/ntcore/manualTests/native/rpc_local.cpp
index b8352d6..3311463 100644
--- a/ntcore/manualTests/native/rpc_local.cpp
+++ b/ntcore/manualTests/native/rpc_local.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -34,12 +34,13 @@
 
 int main() {
   auto inst = nt::GetDefaultInstance();
-  nt::AddLogger(inst,
-                [](const nt::LogMessage& msg) {
-                  std::fputs(msg.message.c_str(), stderr);
-                  std::fputc('\n', stderr);
-                },
-                0, UINT_MAX);
+  nt::AddLogger(
+      inst,
+      [](const nt::LogMessage& msg) {
+        std::fputs(msg.message.c_str(), stderr);
+        std::fputc('\n', stderr);
+      },
+      0, UINT_MAX);
 
   nt::StartServer(inst, "rpc_local.ini", "", 10000);
   auto entry = nt::GetEntry(inst, "func1");

ntcore/manualTests/native/server.cpp

diff --git a/ntcore/manualTests/native/server.cpp b/ntcore/manualTests/native/server.cpp
index 9513bf8..087ddb4 100644
--- a/ntcore/manualTests/native/server.cpp
+++ b/ntcore/manualTests/native/server.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,12 +14,13 @@
 
 int main() {
   auto inst = nt::GetDefaultInstance();
-  nt::AddLogger(inst,
-                [](const nt::LogMessage& msg) {
-                  std::fputs(msg.message.c_str(), stderr);
-                  std::fputc('\n', stderr);
-                },
-                0, UINT_MAX);
+  nt::AddLogger(
+      inst,
+      [](const nt::LogMessage& msg) {
+        std::fputs(msg.message.c_str(), stderr);
+        std::fputc('\n', stderr);
+      },
+      0, UINT_MAX);
   nt::StartServer(inst, "persistent.ini", "", 10000);
   std::this_thread::sleep_for(std::chrono::seconds(1));
 

ntcore/src/main/native/cpp/CallbackManager.h

diff --git a/ntcore/src/main/native/cpp/CallbackManager.h b/ntcore/src/main/native/cpp/CallbackManager.h
index 2d9b11d..bf9b000 100644
--- a/ntcore/src/main/native/cpp/CallbackManager.h
+++ b/ntcore/src/main/native/cpp/CallbackManager.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -83,7 +83,7 @@
     wpi::mutex poll_mutex;
     wpi::condition_variable poll_cond;
     bool terminating = false;
-    bool cancelling = false;
+    bool canceling = false;
   };
   wpi::UidVector<std::shared_ptr<Poller>, 64> m_pollers;
 
@@ -247,10 +247,10 @@
     *timed_out = false;
     while (poller->poll_queue.empty()) {
       if (poller->terminating) return infos;
-      if (poller->cancelling) {
+      if (poller->canceling) {
         // Note: this only works if there's a single thread calling this
         // function for any particular poller, but that's the intended use.
-        poller->cancelling = false;
+        poller->canceling = false;
         return infos;
       }
       if (timeout == 0) {
@@ -287,7 +287,7 @@
 
     {
       std::scoped_lock lock(poller->poll_mutex);
-      poller->cancelling = true;
+      poller->canceling = true;
     }
     poller->poll_cond.notify_one();
   }

ntcore/src/main/native/cpp/Dispatcher.cpp

diff --git a/ntcore/src/main/native/cpp/Dispatcher.cpp b/ntcore/src/main/native/cpp/Dispatcher.cpp
index 6d1ead0..f3bff4d 100644
--- a/ntcore/src/main/native/cpp/Dispatcher.cpp
+++ b/ntcore/src/main/native/cpp/Dispatcher.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -244,6 +244,8 @@
 bool DispatcherBase::IsConnected() const {
   if (!m_active) return false;
 
+  if (m_networkMode == NT_NET_MODE_LOCAL) return true;
+
   std::scoped_lock lock(m_user_mutex);
   for (auto& conn : m_connections) {
     if (conn->state() == NetworkConnection::kActive) return true;

ntcore/src/main/native/cpp/NetworkConnection.cpp

diff --git a/ntcore/src/main/native/cpp/NetworkConnection.cpp b/ntcore/src/main/native/cpp/NetworkConnection.cpp
index 8cc8312..281b638 100644
--- a/ntcore/src/main/native/cpp/NetworkConnection.cpp
+++ b/ntcore/src/main/native/cpp/NetworkConnection.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -135,18 +135,18 @@
   WireDecoder decoder(is, m_proto_rev, m_logger);
 
   set_state(kHandshake);
-  if (!m_handshake(*this,
-                   [&] {
-                     decoder.set_proto_rev(m_proto_rev);
-                     auto msg = Message::Read(decoder, m_get_entry_type);
-                     if (!msg && decoder.error())
-                       DEBUG0(
-                           "error reading in handshake: " << decoder.error());
-                     return msg;
-                   },
-                   [&](wpi::ArrayRef<std::shared_ptr<Message>> msgs) {
-                     m_outgoing.emplace(msgs);
-                   })) {
+  if (!m_handshake(
+          *this,
+          [&] {
+            decoder.set_proto_rev(m_proto_rev);
+            auto msg = Message::Read(decoder, m_get_entry_type);
+            if (!msg && decoder.error())
+              DEBUG0("error reading in handshake: " << decoder.error());
+            return msg;
+          },
+          [&](wpi::ArrayRef<std::shared_ptr<Message>> msgs) {
+            m_outgoing.emplace(msgs);
+          })) {
     set_state(kDead);
     m_active = false;
     goto done;

ntcore/src/main/native/cpp/Storage.cpp

diff --git a/ntcore/src/main/native/cpp/Storage.cpp b/ntcore/src/main/native/cpp/Storage.cpp
index cf0d26c..dadb8e7 100644
--- a/ntcore/src/main/native/cpp/Storage.cpp
+++ b/ntcore/src/main/native/cpp/Storage.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -1053,14 +1053,15 @@
     conn_info.last_update = wpi::Now();
     conn_info.protocol_version = 0x0300;
     unsigned int call_uid = msg->seq_num_uid();
-    m_rpc_server.ProcessRpc(local_id, call_uid, name, msg->str(), conn_info,
-                            [=](StringRef result) {
-                              std::scoped_lock lock(m_mutex);
-                              m_rpc_results.insert(std::make_pair(
-                                  RpcIdPair{local_id, call_uid}, result));
-                              m_rpc_results_cond.notify_all();
-                            },
-                            rpc_uid);
+    m_rpc_server.ProcessRpc(
+        local_id, call_uid, name, msg->str(), conn_info,
+        [=](StringRef result) {
+          std::scoped_lock lock(m_mutex);
+          m_rpc_results.insert(
+              std::make_pair(RpcIdPair{local_id, call_uid}, result));
+          m_rpc_results_cond.notify_all();
+        },
+        rpc_uid);
   } else {
     auto dispatcher = m_dispatcher;
     lock.unlock();

ntcore/src/main/native/cpp/networktables/NetworkTable.cpp

diff --git a/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp b/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp
index 6d41976..ca6e8a6 100644
--- a/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp
+++ b/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -233,13 +233,13 @@
                                                 unsigned int flags) const {
   size_t prefix_len = m_path.size() + 1;
   auto entry = GetEntry(key);
-  return nt::AddEntryListener(entry.GetHandle(),
-                              [=](const EntryNotification& event) {
-                                listener(const_cast<NetworkTable*>(this),
-                                         event.name.substr(prefix_len), entry,
-                                         event.value, event.flags);
-                              },
-                              flags);
+  return nt::AddEntryListener(
+      entry.GetHandle(),
+      [=](const EntryNotification& event) {
+        listener(const_cast<NetworkTable*>(this), event.name.substr(prefix_len),
+                 entry, event.value, event.flags);
+      },
+      flags);
 }
 
 void NetworkTable::RemoveEntryListener(NT_EntryListener listener) const {

ntcore/src/main/native/cpp/ntcore_c.cpp

diff --git a/ntcore/src/main/native/cpp/ntcore_c.cpp b/ntcore/src/main/native/cpp/ntcore_c.cpp
index 60745fe..ee8eabc 100644
--- a/ntcore/src/main/native/cpp/ntcore_c.cpp
+++ b/ntcore/src/main/native/cpp/ntcore_c.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -260,27 +260,29 @@
                                      size_t prefix_len, void* data,
                                      NT_EntryListenerCallback callback,
                                      unsigned int flags) {
-  return nt::AddEntryListener(inst, StringRef(prefix, prefix_len),
-                              [=](const EntryNotification& event) {
-                                NT_EntryNotification c_event;
-                                ConvertToC(event, &c_event);
-                                callback(data, &c_event);
-                                DisposeEntryNotification(&c_event);
-                              },
-                              flags);
+  return nt::AddEntryListener(
+      inst, StringRef(prefix, prefix_len),
+      [=](const EntryNotification& event) {
+        NT_EntryNotification c_event;
+        ConvertToC(event, &c_event);
+        callback(data, &c_event);
+        DisposeEntryNotification(&c_event);
+      },
+      flags);
 }
 
 NT_EntryListener NT_AddEntryListenerSingle(NT_Entry entry, void* data,
                                            NT_EntryListenerCallback callback,
                                            unsigned int flags) {
-  return nt::AddEntryListener(entry,
-                              [=](const EntryNotification& event) {
-                                NT_EntryNotification c_event;
-                                ConvertToC(event, &c_event);
-                                callback(data, &c_event);
-                                DisposeEntryNotification(&c_event);
-                              },
-                              flags);
+  return nt::AddEntryListener(
+      entry,
+      [=](const EntryNotification& event) {
+        NT_EntryNotification c_event;
+        ConvertToC(event, &c_event);
+        callback(data, &c_event);
+        DisposeEntryNotification(&c_event);
+      },
+      flags);
 }
 
 NT_EntryListenerPoller NT_CreateEntryListenerPoller(NT_Inst inst) {
@@ -335,14 +337,15 @@
 NT_ConnectionListener NT_AddConnectionListener(
     NT_Inst inst, void* data, NT_ConnectionListenerCallback callback,
     NT_Bool immediate_notify) {
-  return nt::AddConnectionListener(inst,
-                                   [=](const ConnectionNotification& event) {
-                                     NT_ConnectionNotification event_c;
-                                     ConvertToC(event, &event_c);
-                                     callback(data, &event_c);
-                                     DisposeConnectionNotification(&event_c);
-                                   },
-                                   immediate_notify != 0);
+  return nt::AddConnectionListener(
+      inst,
+      [=](const ConnectionNotification& event) {
+        NT_ConnectionNotification event_c;
+        ConvertToC(event, &event_c);
+        callback(data, &event_c);
+        DisposeConnectionNotification(&event_c);
+      },
+      immediate_notify != 0);
 }
 
 NT_ConnectionListenerPoller NT_CreateConnectionListenerPoller(NT_Inst inst) {
@@ -639,14 +642,15 @@
 
 NT_Logger NT_AddLogger(NT_Inst inst, void* data, NT_LogFunc func,
                        unsigned int min_level, unsigned int max_level) {
-  return nt::AddLogger(inst,
-                       [=](const LogMessage& msg) {
-                         NT_LogMessage msg_c;
-                         ConvertToC(msg, &msg_c);
-                         func(data, &msg_c);
-                         NT_DisposeLogMessage(&msg_c);
-                       },
-                       min_level, max_level);
+  return nt::AddLogger(
+      inst,
+      [=](const LogMessage& msg) {
+        NT_LogMessage msg_c;
+        ConvertToC(msg, &msg_c);
+        func(data, &msg_c);
+        NT_DisposeLogMessage(&msg_c);
+      },
+      min_level, max_level);
 }
 
 NT_LoggerPoller NT_CreateLoggerPoller(NT_Inst inst) {

ntcore/src/main/native/include/networktables/NetworkTable.h

diff --git a/ntcore/src/main/native/include/networktables/NetworkTable.h b/ntcore/src/main/native/include/networktables/NetworkTable.h
index 6504e09..3bfadbd 100644
--- a/ntcore/src/main/native/include/networktables/NetworkTable.h
+++ b/ntcore/src/main/native/include/networktables/NetworkTable.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -176,7 +176,7 @@
   static void SetTeam(int team);
 
   /**
-   * @param address the adress that network tables will connect to in client
+   * @param address the address that network tables will connect to in client
    * mode
    */
   WPI_DEPRECATED(
@@ -774,7 +774,7 @@
 
 }  // namespace nt
 
-// For backwards compatability
+// For backwards compatibility
 #ifndef NAMESPACED_NT
 using nt::NetworkTable;  // NOLINT
 #endif

ntcore/src/main/native/include/networktables/NetworkTableEntry.inl

diff --git a/ntcore/src/main/native/include/networktables/NetworkTableEntry.inl b/ntcore/src/main/native/include/networktables/NetworkTableEntry.inl
index 4a46b96..5bf1dbf 100644
--- a/ntcore/src/main/native/include/networktables/NetworkTableEntry.inl
+++ b/ntcore/src/main/native/include/networktables/NetworkTableEntry.inl
@@ -1,12 +1,16 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2017-2019. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#ifndef NT_ENTRY_INL_
-#define NT_ENTRY_INL_
+#ifndef NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INL_
+#define NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INL_
+
+#include <memory>
+#include <string>
+#include <vector>
 
 namespace nt {
 
@@ -295,4 +299,4 @@
 
 }  // namespace nt
 
-#endif  // NT_ENTRY_INL_
+#endif  // NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INL_

ntcore/src/main/native/include/networktables/NetworkTableInstance.inl

diff --git a/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl b/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
index 2b6607f..5321e83 100644
--- a/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
+++ b/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
@@ -1,12 +1,15 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2017. All Rights Reserved.                             */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#ifndef NT_INSTANCE_INL_
-#define NT_INSTANCE_INL_
+#ifndef NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INL_
+#define NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INL_
+
+#include <utility>
+#include <vector>
 
 namespace nt {
 
@@ -188,4 +191,4 @@
 
 }  // namespace nt
 
-#endif  // NT_INSTANCE_INL_
+#endif  // NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INL_

ntcore/src/main/native/include/networktables/RpcCall.inl

diff --git a/ntcore/src/main/native/include/networktables/RpcCall.inl b/ntcore/src/main/native/include/networktables/RpcCall.inl
index 0e9b522..faf305d 100644
--- a/ntcore/src/main/native/include/networktables/RpcCall.inl
+++ b/ntcore/src/main/native/include/networktables/RpcCall.inl
@@ -1,12 +1,15 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2017. All Rights Reserved.                             */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#ifndef NT_RPCCALL_INL_
-#define NT_RPCCALL_INL_
+#ifndef NTCORE_NETWORKTABLES_RPCCALL_INL_
+#define NTCORE_NETWORKTABLES_RPCCALL_INL_
+
+#include <string>
+#include <utility>
 
 #include "ntcore_cpp.h"
 
@@ -45,4 +48,4 @@
 
 }  // namespace nt
 
-#endif  // NT_RPCCALL_INL_
+#endif  // NTCORE_NETWORKTABLES_RPCCALL_INL_

ntcore/src/main/native/include/ntcore_c.h

diff --git a/ntcore/src/main/native/include/ntcore_c.h b/ntcore/src/main/native/include/ntcore_c.h
index 1ed4025..980752f 100644
--- a/ntcore/src/main/native/include/ntcore_c.h
+++ b/ntcore/src/main/native/include/ntcore_c.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -1747,7 +1747,7 @@
  * It is the caller's responsibility to free the array once its no longer
  * needed. The NT_FreeStringArray() function is useful for this purpose.
  * The returned array is a copy of the array in the value, and must be
- * freed seperately. Note that the individual NT_Strings should not be freed,
+ * freed separately. Note that the individual NT_Strings should not be freed,
  * but the entire array should be freed at once. The NT_FreeStringArray()
  * function will free all the NT_Strings.
  */

ntcore/src/test/native/cpp/ValueTest.cpp

diff --git a/ntcore/src/test/native/cpp/ValueTest.cpp b/ntcore/src/test/native/cpp/ValueTest.cpp
index 818cdac..57d1c38 100644
--- a/ntcore/src/test/native/cpp/ValueTest.cpp
+++ b/ntcore/src/test/native/cpp/ValueTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -241,7 +241,11 @@
   NT_DisposeValue(&cv);
 }
 
+#ifdef NDEBUG
+TEST_F(ValueDeathTest, DISABLED_GetAssertions) {
+#else
 TEST_F(ValueDeathTest, GetAssertions) {
+#endif
   Value v;
   ASSERT_DEATH((void)v.GetBoolean(), "type == NT_BOOLEAN");
   ASSERT_DEATH((void)v.GetDouble(), "type == NT_DOUBLE");

ntcore/src/test/native/cpp/main.cpp

diff --git a/ntcore/src/test/native/cpp/main.cpp b/ntcore/src/test/native/cpp/main.cpp
index d0b0e3c..112289f 100644
--- a/ntcore/src/test/native/cpp/main.cpp
+++ b/ntcore/src/test/native/cpp/main.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,12 +11,13 @@
 #include "ntcore.h"
 
 int main(int argc, char** argv) {
-  nt::AddLogger(nt::GetDefaultInstance(),
-                [](const nt::LogMessage& msg) {
-                  std::fputs(msg.message.c_str(), stderr);
-                  std::fputc('\n', stderr);
-                },
-                0, UINT_MAX);
+  nt::AddLogger(
+      nt::GetDefaultInstance(),
+      [](const nt::LogMessage& msg) {
+        std::fputs(msg.message.c_str(), stderr);
+        std::fputc('\n', stderr);
+      },
+      0, UINT_MAX);
   ::testing::InitGoogleMock(&argc, argv);
   int ret = RUN_ALL_TESTS();
   return ret;

settings.gradle

diff --git a/settings.gradle b/settings.gradle
index 084cf5e..2d59b6e 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -20,21 +20,22 @@
 include 'ntcore'
 include 'hal'
 include 'cscore'
+include 'wpigui'
+include 'wpimath'
 include 'wpilibc'
 include 'wpilibcExamples'
 include 'wpilibcIntegrationTests'
 include 'wpilibjExamples'
 include 'wpilibjIntegrationTests'
 include 'wpilibj'
-include 'simulation:halsim_print'
-include 'simulation:halsim_lowfi'
-include 'simulation:halsim_ds_nt'
 include 'simulation:gz_msgs'
 include 'simulation:frc_gazebo_plugins'
 include 'simulation:halsim_gazebo'
-include 'simulation:lowfi_simulation'
 include 'simulation:halsim_ds_socket'
 include 'simulation:halsim_gui'
+include 'simulation:halsim_ws_core'
+include 'simulation:halsim_ws_client'
+include 'simulation:halsim_ws_server'
 include 'cameraserver'
 include 'cameraserver:multiCameraServer'
 include 'wpilibOldCommands'

shared/config.gradle

diff --git a/shared/config.gradle b/shared/config.gradle
index 7952018..0f8b8a9 100644
--- a/shared/config.gradle
+++ b/shared/config.gradle
@@ -9,17 +9,28 @@
     configureDependencies {
       wpiVersion = "-1"
       niLibVersion = "2020.10.1"
-      opencvVersion = "3.4.7-2"
-      googleTestVersion = "1.9.0-4-437e100-1"
-      imguiVersion = "1.72b-2"
+      opencvVersion = "3.4.7-5"
+      googleTestVersion = "1.9.0-5-437e100-1"
+      imguiVersion = "1.76-9"
+      wpimathVersion = "-1"
     }
   }
 }
 
 nativeUtils.wpi.addWarnings()
 nativeUtils.wpi.addWarningsAsErrors()
+nativeUtils.wpi.addReleaseSymbolGeneration()
 
 nativeUtils.setSinglePrintPerPlatform()
+nativeUtils.enableSourceLink()
+
+nativeUtils.platformConfigs.each {
+    if (it.name.contains('windows')) return
+    it.linker.args << '-Wl,-rpath,\'$ORIGIN\''
+    if (it.name == 'osxx86-64') {
+        it.linker.args << "-headerpad_max_install_names"
+    }
+}
 
 model {
     components {
@@ -138,6 +149,14 @@
                 task.from(binary.staticLibraryFile) {
                     into nativeUtils.getPlatformPath(binary) + '/static'
                 }
+                def pdbDir = binary.staticLibraryFile.parentFile
+                task.from(pdbDir) {
+                    include '*.pdb'
+                    into nativeUtils.getPlatformPath(binary) + '/static'
+                }
+                task.from(new File(pdbDir, "SourceLink.json")) {
+                    into nativeUtils.getPlatformPath(binary) + '/static'
+                }
             }
         }
     }

shared/java/javastyle.gradle

diff --git a/shared/java/javastyle.gradle b/shared/java/javastyle.gradle
index eec3796..ecd551b 100644
--- a/shared/java/javastyle.gradle
+++ b/shared/java/javastyle.gradle
@@ -18,3 +18,8 @@
         ruleSets = []
     }
 }
+
+task javaFormat {
+    dependsOn(tasks.withType(Checkstyle))
+    dependsOn(tasks.withType(Pmd))
+}

shared/javacpp/publish.gradle

diff --git a/shared/javacpp/publish.gradle b/shared/javacpp/publish.gradle
index 5002def..140ae20 100644
--- a/shared/javacpp/publish.gradle
+++ b/shared/javacpp/publish.gradle
@@ -6,7 +6,7 @@
 def artifactGroupId = "edu.wpi.first.${nativeName}"
 def zipBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-cpp_CLS"
 
-def licenseFile = file("$rootDir/license.txt")
+def licenseFile = file("$rootDir/license.md")
 
 task cppSourcesZip(type: Zip) {
     destinationDirectory = outputsFolder

shared/jni/publish.gradle

diff --git a/shared/jni/publish.gradle b/shared/jni/publish.gradle
index 765302a..f369631 100644
--- a/shared/jni/publish.gradle
+++ b/shared/jni/publish.gradle
@@ -7,8 +7,9 @@
 def artifactGroupId = "edu.wpi.first.${nativeName}"
 def zipBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-cpp_CLS"
 def jniBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jni_CLS"
+def jniCvStaticBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jnicvstatic_CLS"
 
-def licenseFile = file("$rootDir/license.txt")
+def licenseFile = file("$rootDir/license.md")
 
 task cppSourcesZip(type: Zip) {
     destinationDirectory = outputsFolder
@@ -110,5 +111,39 @@
                 version wpilibVersioning.version.get()
             }
         }
+
+        if (project.hasProperty('cvStaticBuild') && project.getProperty('cvStaticBuild') == true) {
+            def jniCvTaskList = createComponentZipTasks($.components, ["${nativeName}JNICvStatic"], jniCvStaticBaseName, Zip, project, { task, value ->
+                value.each { binary ->
+                    if (binary.buildable) {
+                        if (binary instanceof SharedLibraryBinarySpec) {
+                            task.dependsOn binary.tasks.link
+                            task.inputs.file(binary.sharedLibraryFile)
+                            task.from(binary.sharedLibraryFile) {
+                                into nativeUtils.getPlatformPath(binary) + '/shared'
+                            }
+                            def sharedPath = binary.sharedLibraryFile.absolutePath
+                            sharedPath = sharedPath.substring(0, sharedPath.length() - 4)
+
+                            task.from(new File(sharedPath + '.pdb')) {
+                                into nativeUtils.getPlatformPath(binary) + '/shared'
+                            }
+                        }
+                    }
+                }
+            })
+
+            publications {
+                jniCvStatic(MavenPublication) {
+                    jniCvTaskList.each {
+                        artifact it
+                    }
+
+                    artifactId = "${baseArtifactId}-jnicvstatic"
+                    groupId artifactGroupId
+                    version wpilibVersioning.version.get()
+                }
+            }
+        }
     }
 }

shared/jni/setupBuild.gradle

diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle
index 6c6148c..369422d 100644
--- a/shared/jni/setupBuild.gradle
+++ b/shared/jni/setupBuild.gradle
@@ -43,7 +43,7 @@
                     source {
                         srcDirs 'src/main/native/cpp'
                         include '**/*.cpp'
-                        exclude '**/jni/*.cpp'
+                        exclude '**/jni/**/*.cpp'
                     }
                     exportedHeaders {
                         srcDir 'src/main/native/include'
@@ -99,7 +99,7 @@
                 baseName = nativeName + 'jni'
             }
 
-            enableCheckTask true
+            enableCheckTask !project.hasProperty('skipJniCheck')
             javaCompileTasks << compileJava
             jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio)
             jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.raspbian)
@@ -108,7 +108,7 @@
                 cpp {
                     source {
                         srcDirs 'src/main/native/cpp'
-                        include '**/jni/*.cpp'
+                        include '**/jni/**/*.cpp'
                     }
                     exportedHeaders {
                         srcDir 'src/main/native/include'
@@ -141,7 +141,7 @@
                 baseName = nativeName + 'jni'
             }
 
-            enableCheckTask true
+            enableCheckTask !project.hasProperty('skipJniCheck')
             javaCompileTasks << compileJava
             jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio)
             jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.raspbian)
@@ -150,7 +150,7 @@
                 cpp {
                     source {
                         srcDirs 'src/main/native/cpp'
-                        include '**/jni/*.cpp'
+                        include '**/jni/**/*.cpp'
                     }
                     exportedHeaders {
                         srcDir 'src/main/native/include'

shared/opencv.gradle

diff --git a/shared/opencv.gradle b/shared/opencv.gradle
index ca3764d..86151c1 100644
--- a/shared/opencv.gradle
+++ b/shared/opencv.gradle
@@ -1,4 +1,4 @@
-def opencvVersion = '3.4.7-2'
+def opencvVersion = '3.4.7-5'
 
 if (project.hasProperty('useCpp') && project.useCpp) {
     model {
@@ -22,12 +22,12 @@
 
 if (project.hasProperty('useJava') && project.useJava) {
     dependencies {
-        implementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
+        implementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}"
         if (!project.hasProperty('skipDev') || !project.skipDev) {
-            devImplementation "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}"
+            devImplementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}"
         }
         if (project.hasProperty('useDocumentation') && project.useDocumentation) {
-            javaSource "edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:${opencvVersion}:sources"
+            javaSource "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}:sources"
         }
     }
 }

simulation/CMakeLists.txt

diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt
index cd3f0fe..912d7b0 100644
--- a/simulation/CMakeLists.txt
+++ b/simulation/CMakeLists.txt
@@ -1,9 +1,10 @@
-add_subdirectory(halsim_gui)
-add_subdirectory(halsim_print)
-add_subdirectory(halsim_lowfi)
-add_subdirectory(halsim_ds_nt)
+if (WITH_GUI)
+    add_subdirectory(halsim_gui)
+endif()
 #add_subdirectory(gz_msgs)
 #add_subdirectory(frc_gazebo_plugins)
 #add_subdirectory(halsim_gazebo)
-#add_subdirectory(lowfi_simulation)
 add_subdirectory(halsim_ds_socket)
+add_subdirectory(halsim_ws_core)
+add_subdirectory(halsim_ws_client)
+add_subdirectory(halsim_ws_server)

simulation/frc_gazebo_plugins/build.gradle

diff --git a/simulation/frc_gazebo_plugins/build.gradle b/simulation/frc_gazebo_plugins/build.gradle
index 918094f..d9db2b9 100644
--- a/simulation/frc_gazebo_plugins/build.gradle
+++ b/simulation/frc_gazebo_plugins/build.gradle
@@ -21,16 +21,14 @@
     gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
 } catch(Exception ex) { }
 
-if (!gazebo_version?.trim()) {
-    println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
-    if (project.hasProperty("makeSim")) {
-        /* Force the build even though we did not find protobuf. */
+if (project.hasProperty("makeSim")) {
+    if (!gazebo_version?.trim()) {
+        println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
         println "makeSim set. Forcing build - failure likely."
     }
-    else {
-        ext.skip_frc_plugins = true
-        println "Skipping FRC Plugins."
-    }
+} else {
+    ext.skip_frc_plugins = true
+    println "Skipping FRC Plugins."
 }
 
 evaluationDependsOn(":simulation:gz_msgs")

simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy

diff --git a/simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy b/simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy
index 887c9c1..a2ebdd2 100644
--- a/simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy
+++ b/simulation/frc_gazebo_plugins/frc_gazebo_plugins.doxy
@@ -1947,7 +1947,7 @@
 EXPAND_AS_DEFINED      =
 
 # If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
-# remove all refrences to function-like macros that are alone on a line, have an
+# remove all references to function-like macros that are alone on a line, have an
 # all uppercase name, and do not end with a semicolon. Such function macros are
 # typically used for boiler-plate code, and will confuse the parser if not
 # removed.

simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h

diff --git a/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h b/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h
index c33f708..4ecc17a 100644
--- a/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h
+++ b/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -69,7 +69,7 @@
   /// \brief Whether or not this encoder measures radians or degrees.
   bool radians;
 
-  /// \brief A factor to mulitply this output by.
+  /// \brief A factor to multiply this output by.
   double multiplier;
 
   /// \brief Whether or not the encoder has been stopped.

simulation/gz_msgs/build.gradle

diff --git a/simulation/gz_msgs/build.gradle b/simulation/gz_msgs/build.gradle
index 353f4ad..d07dc09 100644
--- a/simulation/gz_msgs/build.gradle
+++ b/simulation/gz_msgs/build.gradle
@@ -26,17 +26,14 @@
 } catch(Exception ex) {
 }
 
-if (!protobuf_version?.trim()) {
-    println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
-    protobuf_version = "+"
-    if (project.hasProperty("makeSim")) {
-        /* Force the build even though we did not find protobuf. */
+if (project.hasProperty("makeSim")) {
+    if (!protobuf_version?.trim()) {
+        println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
         println "makeSim set. Forcing build - failure likely."
     }
-    else {
-        ext.skip_gz_msgs = true
-        println "Skipping gz_msgs."
-    }
+} else {
+    ext.skip_gz_msgs = true
+    println "Skipping gz msgs."
 }
 
 tasks.whenTaskAdded { task ->
@@ -88,6 +85,11 @@
             binaries {
                 all {
                     cppCompiler.args "-fPIC"
+
+                    // Disable -Wzero-length-array on Clang
+                    if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                        it.cppCompiler.args.add('-Wno-error=zero-length-array')
+                    }
                 }
             }
         }

simulation/halsim_ds_nt/CMakeLists.txt

diff --git a/simulation/halsim_ds_nt/CMakeLists.txt b/simulation/halsim_ds_nt/CMakeLists.txt
deleted file mode 100644
index bfed6ff..0000000
--- a/simulation/halsim_ds_nt/CMakeLists.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-project(halsim_ds_nt)
-
-include(CompileWarnings)
-
-file(GLOB halsim_ds_nt_src src/main/native/cpp/*.cpp)
-
-add_library(halsim_ds_nt MODULE ${halsim_ds_nt_src})
-wpilib_target_warnings(halsim_ds_nt)
-set_target_properties(halsim_ds_nt PROPERTIES DEBUG_POSTFIX "d")
-target_link_libraries(halsim_ds_nt PUBLIC hal ntcore)
-
-target_include_directories(halsim_ds_nt PRIVATE src/main/native/include)
-
-set_property(TARGET halsim_ds_nt PROPERTY FOLDER "libraries")
-
-install(TARGETS halsim_ds_nt EXPORT halsim_ds_nt DESTINATION "${main_lib_dest}")

simulation/halsim_ds_nt/build.gradle

diff --git a/simulation/halsim_ds_nt/build.gradle b/simulation/halsim_ds_nt/build.gradle
deleted file mode 100644
index fa02630..0000000
--- a/simulation/halsim_ds_nt/build.gradle
+++ /dev/null
@@ -1,9 +0,0 @@
-description = "A simulation shared object that uses NetworkTables to act as a stand-in for the FRC Driver Station"
-
-ext {
-    includeNtCore = true
-    includeWpiutil = true
-    pluginName = 'halsim_ds_nt'
-}
-
-apply from: "${rootDir}/shared/plugins/setupBuild.gradle"

simulation/halsim_ds_nt/src/dev/native/cpp/main.cpp

diff --git a/simulation/halsim_ds_nt/src/dev/native/cpp/main.cpp b/simulation/halsim_ds_nt/src/dev/native/cpp/main.cpp
deleted file mode 100644
index e324b44..0000000
--- a/simulation/halsim_ds_nt/src/dev/native/cpp/main.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-int main() {}

simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp

diff --git a/simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp b/simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp
deleted file mode 100644
index d694530..0000000
--- a/simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp
+++ /dev/null
@@ -1,194 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "HALSimDsNt.h"
-
-void HALSimDSNT::Initialize() {
-  rootTable =
-      nt::NetworkTableInstance::GetDefault().GetTable("sim")->GetSubTable(
-          "DS_CONTROL");  // Not to be confused with sim::DriverStation from
-                          // HALSim LowFi
-
-  // LOOP TIMING //
-
-  auto timinghz = rootTable->GetEntry("timing_hz");
-  timinghz.ForceSetDouble(50);
-  timinghz.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        double valIn = ev.value->GetDouble();
-        double val = 0;
-        val = (valIn < 1 ? 1 : valIn > 100 ? 100 : valIn);
-
-        if (val != valIn) {
-          this->rootTable->GetEntry("timing_hz").ForceSetDouble(val);
-          Flush();
-        }
-
-        this->timingHz = val;
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  // MODES //
-
-  modeTable = rootTable->GetSubTable("mode");
-  auto mtele = modeTable->GetEntry("teleop?");
-  auto mauto = modeTable->GetEntry("auto?");
-  auto mtest = modeTable->GetEntry("test?");
-  auto enabled = modeTable->GetEntry("enabled?");
-  auto estop = modeTable->GetEntry("estop?");
-
-  mtele.ForceSetBoolean(true);
-  mauto.ForceSetBoolean(false);
-  mtest.ForceSetBoolean(false);
-  enabled.ForceSetBoolean(false);
-  estop.ForceSetBoolean(false);
-
-  mtele.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        this->HandleModePress(HALSimDSNT_Mode::teleop, ev.value->GetBoolean());
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  mauto.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        this->HandleModePress(HALSimDSNT_Mode::auton, ev.value->GetBoolean());
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  mtest.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        this->HandleModePress(HALSimDSNT_Mode::test, ev.value->GetBoolean());
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  enabled.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        std::scoped_lock lock(modeMutex);
-        if (!this->isEstop) {
-          this->isEnabled = ev.value->GetBoolean();
-        } else {
-          this->isEnabled = false;
-        }
-        this->DoModeUpdate();
-        this->UpdateModeButtons();
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  estop.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        std::scoped_lock lock(modeMutex);
-        this->isEstop = ev.value->GetBoolean();
-        if (this->isEstop) {
-          this->isEnabled = false;
-        }
-        this->DoModeUpdate();
-        this->UpdateModeButtons();
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  // ALLIANCES //
-
-  allianceTable = rootTable->GetSubTable("alliance");
-  auto allianceStation = allianceTable->GetEntry("station");
-  auto allianceColorRed = allianceTable->GetEntry("red?");
-
-  allianceStation.ForceSetDouble(1);
-  allianceColorRed.ForceSetBoolean(true);
-
-  allianceStation.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        double stnIn = ev.value->GetDouble();
-        double stn = 0;
-        stn = (stnIn > 3 ? 3 : stnIn < 1 ? 1 : stnIn);
-
-        if (stn != stnIn) {
-          this->allianceTable->GetEntry("station").ForceSetDouble(stn);
-          Flush();
-        }
-
-        this->allianceStation = stn;
-        this->DoAllianceUpdate();
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  allianceColorRed.AddListener(
-      [this](const nt::EntryNotification& ev) -> void {
-        this->isAllianceRed = ev.value->GetBoolean();
-        this->DoAllianceUpdate();
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  // FINAL LOGIC //
-
-  Flush();
-
-  loopThread = std::thread([this]() -> void {
-    this->running = true;
-    this->LoopFunc();
-  });
-  loopThread.detach();
-}
-
-void HALSimDSNT::HandleModePress(enum HALSimDSNT_Mode mode, bool isPressed) {
-  if (isPressed) {
-    if (mode != currentMode) {
-      std::scoped_lock lock(modeMutex);
-      currentMode = mode;
-      isEnabled = false;
-      this->DoModeUpdate();
-    }
-  }
-
-  this->UpdateModeButtons();
-}
-
-void HALSimDSNT::UpdateModeButtons() {
-  modeTable->GetEntry("teleop?").ForceSetBoolean(currentMode ==
-                                                 HALSimDSNT_Mode::teleop);
-  modeTable->GetEntry("auto?").ForceSetBoolean(currentMode ==
-                                               HALSimDSNT_Mode::auton);
-  modeTable->GetEntry("test?").ForceSetBoolean(currentMode ==
-                                               HALSimDSNT_Mode::test);
-  modeTable->GetEntry("enabled?").ForceSetBoolean(isEnabled);
-  Flush();
-}
-
-void HALSimDSNT::DoModeUpdate() {
-  HALSIM_SetDriverStationAutonomous(currentMode == HALSimDSNT_Mode::auton);
-  HALSIM_SetDriverStationTest(currentMode == HALSimDSNT_Mode::test);
-  HALSIM_SetDriverStationEnabled(isEnabled);
-  if (isEnabled && !lastIsEnabled) {
-    currentMatchTime = 0;
-  }
-  lastIsEnabled = isEnabled;
-  HALSIM_SetDriverStationEStop(isEstop);
-  HALSIM_SetDriverStationFmsAttached(false);
-  HALSIM_SetDriverStationDsAttached(true);
-  HALSIM_NotifyDriverStationNewData();
-}
-
-void HALSimDSNT::DoAllianceUpdate() {
-  HALSIM_SetDriverStationAllianceStationId(static_cast<HAL_AllianceStationID>(
-      (isAllianceRed ? HAL_AllianceStationID_kRed1
-                     : HAL_AllianceStationID_kBlue1) +
-      (static_cast<int32_t>(allianceStation) - 1)));
-}
-
-void HALSimDSNT::LoopFunc() {
-  while (running) {
-    double dt = 1000 / timingHz;
-    std::this_thread::sleep_for(
-        std::chrono::milliseconds(static_cast<int64_t>(dt)));
-    if (isEnabled) {
-      currentMatchTime = currentMatchTime + dt;
-      HALSIM_SetDriverStationMatchTime(currentMatchTime);
-    }
-    HALSIM_NotifyDriverStationNewData();
-  }
-}
-
-void HALSimDSNT::Flush() { rootTable->GetInstance().Flush(); }

simulation/halsim_ds_nt/src/main/native/cpp/main.cpp

diff --git a/simulation/halsim_ds_nt/src/main/native/cpp/main.cpp b/simulation/halsim_ds_nt/src/main/native/cpp/main.cpp
deleted file mode 100644
index 7090de9..0000000
--- a/simulation/halsim_ds_nt/src/main/native/cpp/main.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <iostream>
-
-#include <HALSimDsNt.h>
-
-static HALSimDSNT dsnt;
-
-extern "C" {
-#if defined(WIN32) || defined(_WIN32)
-__declspec(dllexport)
-#endif
-    int HALSIM_InitExtension(void) {
-  std::cout << "DriverStationNT Initializing." << std::endl;
-
-  dsnt.Initialize();
-
-  std::cout << "DriverStationNT Initialized!" << std::endl;
-  return 0;
-}
-}  // extern "C"

simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h

diff --git a/simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h b/simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h
deleted file mode 100644
index 21d19ee..0000000
--- a/simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <atomic>
-#include <memory>
-#include <thread>
-
-#include <mockdata/DriverStationData.h>
-#include <networktables/NetworkTableInstance.h>
-#include <wpi/mutex.h>
-
-enum HALSimDSNT_Mode { teleop, auton, test };
-
-class HALSimDSNT {
- public:
-  std::shared_ptr<nt::NetworkTable> rootTable, modeTable, allianceTable;
-  enum HALSimDSNT_Mode currentMode;
-  bool isEnabled, lastIsEnabled, isEstop;
-  std::atomic<bool> isAllianceRed, running;
-  std::atomic<double> currentMatchTime, timingHz, allianceStation;
-  std::thread loopThread;
-  wpi::mutex modeMutex;
-
-  void Initialize();
-  void HandleModePress(enum HALSimDSNT_Mode mode, bool isPressed);
-  void UpdateModeButtons();
-  void DoModeUpdate();
-  void DoAllianceUpdate();
-  void LoopFunc();
-  void Flush();
-};

simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp

diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
index 95f393b..4a87f13 100644
--- a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
+++ b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,12 +10,11 @@
 #include <algorithm>
 #include <chrono>
 #include <cstring>
-#include <iostream>
 #include <thread>
 #include <vector>
 
-#include <mockdata/DriverStationData.h>
-#include <mockdata/MockHooks.h>
+#include <hal/simulation/DriverStationData.h>
+#include <hal/simulation/MockHooks.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/Format.h>
 

simulation/halsim_ds_socket/src/main/native/cpp/main.cpp

diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
index 7d683b3..bafe8b3 100644
--- a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
+++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,10 +16,11 @@
 
 #include <sys/types.h>
 
+#include <atomic>
 #include <cstring>
-#include <iostream>
 
 #include <DSCommPacket.h>
+#include <hal/Extensions.h>
 #include <wpi/EventLoopRunner.h>
 #include <wpi/StringRef.h>
 #include <wpi/raw_ostream.h>
@@ -36,6 +37,7 @@
 using namespace wpi::uv;
 
 static std::unique_ptr<Buffer> singleByte;
+static std::atomic<bool> gDSConnected = false;
 
 namespace {
 struct DataStore {
@@ -92,12 +94,16 @@
 
   tcp->Listen([t = tcp.get()] {
     auto client = t->Accept();
+    gDSConnected = true;
 
     client->data.connect([t](Buffer& buf, size_t len) {
       HandleTcpDataStream(buf, len, *t->GetData<DataStore>());
     });
     client->StartRead();
-    client->end.connect([c = client.get()] { c->Close(); });
+    client->end.connect([c = client.get()] {
+      c->Close();
+      gDSConnected = false;
+    });
   });
 }
 
@@ -172,13 +178,14 @@
   static bool once = false;
 
   if (once) {
-    std::cerr << "Error: cannot invoke HALSIM_InitExtension twice."
-              << std::endl;
+    wpi::errs() << "Error: cannot invoke HALSIM_InitExtension twice.\n";
     return -1;
   }
   once = true;
 
-  std::cout << "DriverStationSocket Initializing." << std::endl;
+  wpi::outs() << "DriverStationSocket Initializing.\n";
+
+  HAL_RegisterExtension("ds_socket", &gDSConnected);
 
   singleByte = std::make_unique<Buffer>("0");
 
@@ -186,7 +193,7 @@
 
   eventLoopRunner->ExecAsync(SetupEventLoop);
 
-  std::cout << "DriverStationSocket Initialized!" << std::endl;
+  wpi::outs() << "DriverStationSocket Initialized!\n";
   return 0;
 }
 }  // extern "C"

simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h

diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
index 6f7e0bf..99851f2 100644
--- a/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
+++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 
 namespace halsim {
 
-typedef struct {
+struct DSCommJoystickPacket {
   HAL_JoystickAxes axes;
   HAL_JoystickButtons buttons;
   HAL_JoystickPOVs povs;
@@ -26,6 +26,6 @@
   }
 
   void ResetTcp() { std::memset(&descriptor, 0, sizeof(descriptor)); }
-} DSCommJoystickPacket;
+};
 
 }  // namespace halsim

simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h

diff --git a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
index ebe36e4..f189d6a 100644
--- a/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
+++ b/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,7 @@
 #include <array>
 
 #include <DSCommJoystickPacket.h>
-#include <mockdata/DriverStationData.h>
+#include <hal/simulation/DriverStationData.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/raw_uv_ostream.h>
 

simulation/halsim_gazebo/build.gradle

diff --git a/simulation/halsim_gazebo/build.gradle b/simulation/halsim_gazebo/build.gradle
index 63e8809..d476ef0 100644
--- a/simulation/halsim_gazebo/build.gradle
+++ b/simulation/halsim_gazebo/build.gradle
@@ -19,16 +19,14 @@
     gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
 } catch(Exception ex) { }
 
-if (!gazebo_version?.trim()) {
-    println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
-    if (project.hasProperty("makeSim")) {
-        /* Force the build even though we did not find protobuf. */
+if (project.hasProperty("makeSim")) {
+    if (!gazebo_version?.trim()) {
+        println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
         println "makeSim set. Forcing build - failure likely."
     }
-    else {
-        ext.skip_frc_plugins = true
-        println "Skipping FRC Plugins."
-    }
+} else {
+    ext.skip_frc_plugins = true
+    println "Skipping FRC Plugins."
 }
 
 evaluationDependsOn(":simulation:gz_msgs")

simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp

diff --git a/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp b/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp
index 30319c9..ce20221 100644
--- a/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp
+++ b/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,8 +11,8 @@
 
 #include <hal/Power.h>
 #include <hal/Value.h>
-#include <mockdata/AnalogInData.h>
-#include <mockdata/NotifyListener.h>
+#include <hal/simulation/AnalogInData.h>
+#include <hal/simulation/NotifyListener.h>
 
 static void init_callback(const char* name, void* param,
                           const struct HAL_Value* value) {

simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp

diff --git a/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp b/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp
index d0c190e..f95b4f5 100644
--- a/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp
+++ b/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,8 @@
 #include <string>
 
 #include <hal/Value.h>
-#include <mockdata/DIOData.h>
-#include <mockdata/NotifyListener.h>
+#include <hal/simulation/DIOData.h>
+#include <hal/simulation/NotifyListener.h>
 
 static void init_callback(const char* name, void* param,
                           const struct HAL_Value* value) {

simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp

diff --git a/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp b/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp
index bb5894d..03c2f79 100644
--- a/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp
+++ b/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,8 @@
 #include <string>
 
 #include <hal/Value.h>
-#include <mockdata/EncoderData.h>
-#include <mockdata/NotifyListener.h>
+#include <hal/simulation/EncoderData.h>
+#include <hal/simulation/NotifyListener.h>
 
 static void encoder_init_callback(const char* name, void* param,
                                   const struct HAL_Value* value) {

simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp

diff --git a/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp b/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp
index 0f0a44e..1802f2e 100644
--- a/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp
+++ b/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,8 @@
 #include <string>
 
 #include <hal/Value.h>
-#include <mockdata/NotifyListener.h>
-#include <mockdata/PCMData.h>
+#include <hal/simulation/NotifyListener.h>
+#include <hal/simulation/PCMData.h>
 
 #include "simulation/gz_msgs/msgs.h"
 

simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp

diff --git a/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp b/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp
index 3e01e1d..a297255 100644
--- a/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp
+++ b/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,8 @@
 #include <string>
 
 #include <hal/Value.h>
-#include <mockdata/NotifyListener.h>
-#include <mockdata/PWMData.h>
+#include <hal/simulation/NotifyListener.h>
+#include <hal/simulation/PWMData.h>
 
 #include "simulation/gz_msgs/msgs.h"
 

simulation/halsim_gui/CMakeLists.txt

diff --git a/simulation/halsim_gui/CMakeLists.txt b/simulation/halsim_gui/CMakeLists.txt
index 91af53c..424ff5b 100644
--- a/simulation/halsim_gui/CMakeLists.txt
+++ b/simulation/halsim_gui/CMakeLists.txt
@@ -1,13 +1,16 @@
 project(halsim_gui)
 
 include(CompileWarnings)
+include(LinkMacOSGUI)
 
 file(GLOB halsim_gui_src src/main/native/cpp/*.cpp)
 
 add_library(halsim_gui MODULE ${halsim_gui_src})
 wpilib_target_warnings(halsim_gui)
 set_target_properties(halsim_gui PROPERTIES DEBUG_POSTFIX "d")
-target_link_libraries(halsim_gui PUBLIC hal PRIVATE imgui)
+
+wpilib_link_macos_gui(halsim_gui)
+target_link_libraries(halsim_gui PUBLIC hal ntcore wpimath PRIVATE wpigui)
 
 target_include_directories(halsim_gui PRIVATE src/main/native/include)
 

simulation/halsim_gui/build.gradle

diff --git a/simulation/halsim_gui/build.gradle b/simulation/halsim_gui/build.gradle
index 5df5302..0fd5c2c 100644
--- a/simulation/halsim_gui/build.gradle
+++ b/simulation/halsim_gui/build.gradle
@@ -4,6 +4,7 @@
 
     ext {
         includeWpiutil = true
+        includeNtCore = true
         pluginName = 'halsim_gui'
     }
 
@@ -22,17 +23,19 @@
     model {
         binaries {
             all {
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                 nativeUtils.useRequiredLibrary(it, 'imgui_static')
                 if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
                     it.buildable = false
                     return
                 }
                 if (it.targetPlatform.operatingSystem.isWindows()) {
-                    it.linker.args << 'Gdi32.lib' << 'Shell32.lib'
+                    it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
                 } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
-                    it.linker.args << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo'
+                    it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
                 } else {
-                    it.linker.args << '-lX11' << '-lvulkan'
+                    it.linker.args << '-lX11'
                 }
             }
         }

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.cpp b/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.cpp
index e993ba2..fa772ad 100644
--- a/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,15 +8,35 @@
 #include "AccelerometerGui.h"
 
 #include <cstdio>
+#include <memory>
 
 #include <hal/Value.h>
+#include <hal/simulation/AccelerometerData.h>
 #include <imgui.h>
-#include <mockdata/AccelerometerData.h>
 
+#include "GuiDataSource.h"
+#include "HALSimGui.h"
 #include "SimDeviceGui.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerX, "X Accel");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerY, "Y Accel");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerZ, "Z Accel");
+}  // namespace
+
+static std::unique_ptr<AccelerometerXSource> gAccelXSource;
+static std::unique_ptr<AccelerometerYSource> gAccelYSource;
+static std::unique_ptr<AccelerometerZSource> gAccelZSource;
+
+static void UpdateAccelSources() {
+  if (!HALSIM_GetAccelerometerActive(0)) return;
+  if (!gAccelXSource) gAccelXSource = std::make_unique<AccelerometerXSource>(0);
+  if (!gAccelYSource) gAccelYSource = std::make_unique<AccelerometerYSource>(0);
+  if (!gAccelZSource) gAccelZSource = std::make_unique<AccelerometerZSource>(0);
+}
+
 static void DisplayAccelerometers() {
   if (!HALSIM_GetAccelerometerActive(0)) return;
   if (SimDeviceGui::StartDevice("BuiltInAccel")) {
@@ -28,18 +48,21 @@
     SimDeviceGui::DisplayValue("Range", true, &value, rangeOptions, 3);
 
     // X Accel
-    value = HAL_MakeDouble(HALSIM_GetAccelerometerX(0));
-    if (SimDeviceGui::DisplayValue("X Accel", false, &value))
+    value = HAL_MakeDouble(gAccelXSource->GetValue());
+    if (SimDeviceGui::DisplayValueSource("X Accel", false, &value,
+                                         gAccelXSource.get()))
       HALSIM_SetAccelerometerX(0, value.data.v_double);
 
     // Y Accel
-    value = HAL_MakeDouble(HALSIM_GetAccelerometerY(0));
-    if (SimDeviceGui::DisplayValue("Y Accel", false, &value))
+    value = HAL_MakeDouble(gAccelYSource->GetValue());
+    if (SimDeviceGui::DisplayValueSource("Y Accel", false, &value,
+                                         gAccelYSource.get()))
       HALSIM_SetAccelerometerY(0, value.data.v_double);
 
     // Z Accel
-    value = HAL_MakeDouble(HALSIM_GetAccelerometerZ(0));
-    if (SimDeviceGui::DisplayValue("Z Accel", false, &value))
+    value = HAL_MakeDouble(gAccelZSource->GetValue());
+    if (SimDeviceGui::DisplayValueSource("Z Accel", false, &value,
+                                         gAccelZSource.get()))
       HALSIM_SetAccelerometerZ(0, value.data.v_double);
 
     SimDeviceGui::FinishDevice();
@@ -47,5 +70,6 @@
 }
 
 void AccelerometerGui::Initialize() {
+  HALSimGui::AddExecute(UpdateAccelSources);
   SimDeviceGui::Add(DisplayAccelerometers);
 }

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

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

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.cpp b/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.cpp
index 1ae0fae..5df30e6 100644
--- a/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,38 +8,73 @@
 #include "AnalogGyroGui.h"
 
 #include <cstdio>
+#include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
 #include <hal/Value.h>
+#include <hal/simulation/AnalogGyroData.h>
 #include <imgui.h>
-#include <mockdata/AnalogGyroData.h>
 
+#include "GuiDataSource.h"
+#include "HALSimGui.h"
 #include "SimDeviceGui.h"
 
 using namespace halsimgui;
 
-static void DisplayAnalogGyros() {
-  static int numAccum = HAL_GetNumAccumulators();
-  for (int i = 0; i < numAccum; ++i) {
-    if (!HALSIM_GetAnalogGyroInitialized(i)) continue;
-    char name[32];
-    std::snprintf(name, sizeof(name), "AnalogGyro[%d]", i);
-    if (SimDeviceGui::StartDevice(name)) {
-      HAL_Value value;
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogGyroAngle, "AGyro Angle");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogGyroRate, "AGyro Rate");
+struct AnalogGyroSource {
+  explicit AnalogGyroSource(int32_t index) : angle{index}, rate{index} {}
+  AnalogGyroAngleSource angle;
+  AnalogGyroRateSource rate;
+};
+}  // namespace
 
-      // angle
-      value = HAL_MakeDouble(HALSIM_GetAnalogGyroAngle(i));
-      if (SimDeviceGui::DisplayValue("Angle", false, &value))
-        HALSIM_SetAnalogGyroAngle(i, value.data.v_double);
+static std::vector<std::unique_ptr<AnalogGyroSource>> gAnalogGyroSources;
 
-      // rate
-      value = HAL_MakeDouble(HALSIM_GetAnalogGyroRate(i));
-      if (SimDeviceGui::DisplayValue("Rate", false, &value))
-        HALSIM_SetAnalogGyroRate(i, value.data.v_double);
-
-      SimDeviceGui::FinishDevice();
+static void UpdateAnalogGyroSources() {
+  for (int i = 0, iend = gAnalogGyroSources.size(); i < iend; ++i) {
+    auto& source = gAnalogGyroSources[i];
+    if (HALSIM_GetAnalogGyroInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<AnalogGyroSource>(i);
+      }
+    } else {
+      source.reset();
     }
   }
 }
 
-void AnalogGyroGui::Initialize() { SimDeviceGui::Add(DisplayAnalogGyros); }
+static void DisplayAnalogGyros() {
+  for (int i = 0, iend = gAnalogGyroSources.size(); i < iend; ++i) {
+    if (auto source = gAnalogGyroSources[i].get()) {
+      char name[32];
+      std::snprintf(name, sizeof(name), "AnalogGyro[%d]", i);
+      if (SimDeviceGui::StartDevice(name)) {
+        HAL_Value value;
+
+        // angle
+        value = HAL_MakeDouble(source->angle.GetValue());
+        if (SimDeviceGui::DisplayValueSource("Angle", false, &value,
+                                             &source->angle))
+          HALSIM_SetAnalogGyroAngle(i, value.data.v_double);
+
+        // rate
+        value = HAL_MakeDouble(source->rate.GetValue());
+        if (SimDeviceGui::DisplayValueSource("Rate", false, &value,
+                                             &source->rate))
+          HALSIM_SetAnalogGyroRate(i, value.data.v_double);
+
+        SimDeviceGui::FinishDevice();
+      }
+    }
+  }
+}
+
+void AnalogGyroGui::Initialize() {
+  gAnalogGyroSources.resize(HAL_GetNumAccumulators());
+  HALSimGui::AddExecute(UpdateAnalogGyroSources);
+  SimDeviceGui::Add(DisplayAnalogGyros);
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp b/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
index 0918a69..8ba1459 100644
--- a/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,26 +7,52 @@
 
 #include "AnalogInputGui.h"
 
-#include <cstdio>
+#include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/AnalogGyroData.h>
+#include <hal/simulation/AnalogInData.h>
+#include <hal/simulation/SimDeviceData.h>
 #include <imgui.h>
-#include <mockdata/AnalogGyroData.h>
-#include <mockdata/AnalogInData.h>
-#include <mockdata/SimDeviceData.h>
 
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogInVoltage, "AIn");
+}  // namespace
+
+// indexed by channel
+static IniSaver<NameInfo> gAnalogInputs{"AnalogInput"};
+static std::vector<std::unique_ptr<AnalogInVoltageSource>> gAnalogInputSources;
+
+static void UpdateAnalogInputSources() {
+  for (int i = 0, iend = gAnalogInputSources.size(); i < iend; ++i) {
+    auto& source = gAnalogInputSources[i];
+    if (HALSIM_GetAnalogInInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<AnalogInVoltageSource>(i);
+        source->SetName(gAnalogInputs[i].GetName());
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
 static void DisplayAnalogInputs() {
   ImGui::Text("(Use Ctrl+Click to edit value)");
   bool hasInputs = false;
-  static int numAnalog = HAL_GetNumAnalogInputs();
-  static int numAccum = HAL_GetNumAccumulators();
+  static const int numAccum = HAL_GetNumAccumulators();
   bool first = true;
-  for (int i = 0; i < numAnalog; ++i) {
-    if (HALSIM_GetAnalogInInitialized(i)) {
+  for (int i = 0, iend = gAnalogInputSources.size(); i < iend; ++i) {
+    if (auto source = gAnalogInputSources[i].get()) {
+      ImGui::PushID(i);
       hasInputs = true;
 
       if (!first) {
@@ -36,27 +62,40 @@
         first = false;
       }
 
-      char name[32];
-      std::snprintf(name, sizeof(name), "In[%d]", i);
+      auto& info = gAnalogInputs[i];
+      // build label
+      char label[128];
+      info.GetLabel(label, sizeof(label), "In", i);
+
       if (i < numAccum && HALSIM_GetAnalogGyroInitialized(i)) {
         ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-        ImGui::LabelText(name, "AnalogGyro[%d]", i);
+        ImGui::LabelText(label, "AnalogGyro[%d]", i);
         ImGui::PopStyleColor();
       } else if (auto simDevice = HALSIM_GetAnalogInSimDevice(i)) {
         ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-        ImGui::LabelText(name, "%s", HALSIM_GetSimDeviceName(simDevice));
+        ImGui::LabelText(label, "%s", HALSIM_GetSimDeviceName(simDevice));
         ImGui::PopStyleColor();
       } else {
-        float val = HALSIM_GetAnalogInVoltage(i);
-        if (ImGui::SliderFloat(name, &val, 0.0, 5.0))
+        float val = source->GetValue();
+        if (source->SliderFloat(label, &val, 0.0, 5.0))
           HALSIM_SetAnalogInVoltage(i, val);
       }
+
+      // context menu to change name
+      if (info.PopupEditName(i)) {
+        source->SetName(info.GetName());
+      }
+      ImGui::PopID();
     }
   }
   if (!hasInputs) ImGui::Text("No analog inputs");
 }
 
 void AnalogInputGui::Initialize() {
+  gAnalogInputs.Initialize();
+  gAnalogInputSources.resize(HAL_GetNumAnalogInputs());
+
+  HALSimGui::AddExecute(UpdateAnalogInputSources);
   HALSimGui::AddWindow("Analog Inputs", DisplayAnalogInputs,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("Analog Inputs", 640, 20);

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.cpp b/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.cpp
index 38b69a9..3e7f4ea 100644
--- a/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,41 +7,75 @@
 
 #include "AnalogOutGui.h"
 
-#include <cstdio>
-#include <cstring>
 #include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/AnalogOutData.h>
 #include <imgui.h>
-#include <mockdata/AnalogOutData.h>
 
+#include "GuiDataSource.h"
+#include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 #include "SimDeviceGui.h"
 
 using namespace halsimgui;
 
-static void DisplayAnalogOutputs() {
-  static const int numAnalog = HAL_GetNumAnalogOutputs();
-  static auto init = std::make_unique<bool[]>(numAnalog);
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogOutVoltage, "AOut");
+}  // namespace
 
+static IniSaver<NameInfo> gAnalogOuts{"AnalogOut"};  // indexed by channel
+static std::vector<std::unique_ptr<AnalogOutVoltageSource>> gAnalogOutSources;
+
+static void UpdateAnalogOutSources() {
+  for (int i = 0, iend = gAnalogOutSources.size(); i < iend; ++i) {
+    auto& source = gAnalogOutSources[i];
+    if (HALSIM_GetAnalogOutInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<AnalogOutVoltageSource>(i);
+        source->SetName(gAnalogOuts[i].GetName());
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
+static void DisplayAnalogOutputs() {
   int count = 0;
-  for (int i = 0; i < numAnalog; ++i) {
-    init[i] = HALSIM_GetAnalogOutInitialized(i);
-    if (init[i]) ++count;
+  for (auto&& source : gAnalogOutSources) {
+    if (source) ++count;
   }
 
   if (count == 0) return;
 
   if (SimDeviceGui::StartDevice("Analog Outputs")) {
-    for (int i = 0; i < numAnalog; ++i) {
-      if (!init[i]) continue;
-      char name[32];
-      std::snprintf(name, sizeof(name), "Out[%d]", i);
-      HAL_Value value = HAL_MakeDouble(HALSIM_GetAnalogOutVoltage(i));
-      SimDeviceGui::DisplayValue(name, true, &value);
+    for (int i = 0, iend = gAnalogOutSources.size(); i < iend; ++i) {
+      if (auto source = gAnalogOutSources[i].get()) {
+        ImGui::PushID(i);
+
+        auto& info = gAnalogOuts[i];
+        char label[128];
+        info.GetLabel(label, sizeof(label), "Out", i);
+        HAL_Value value = HAL_MakeDouble(source->GetValue());
+        SimDeviceGui::DisplayValueSource(label, true, &value, source);
+
+        if (info.PopupEditName(i)) {
+          if (source) source->SetName(info.GetName());
+        }
+        ImGui::PopID();
+      }
     }
 
     SimDeviceGui::FinishDevice();
   }
 }
 
-void AnalogOutGui::Initialize() { SimDeviceGui::Add(DisplayAnalogOutputs); }
+void AnalogOutGui::Initialize() {
+  gAnalogOuts.Initialize();
+  gAnalogOutSources.resize(HAL_GetNumAnalogOutputs());
+  HALSimGui::AddExecute(UpdateAnalogOutSources);
+  SimDeviceGui::Add(DisplayAnalogOutputs);
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/CompressorGui.cpp b/simulation/halsim_gui/src/main/native/cpp/CompressorGui.cpp
index 737a6f9..43215fa 100644
--- a/simulation/halsim_gui/src/main/native/cpp/CompressorGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/CompressorGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,56 +8,97 @@
 #include "CompressorGui.h"
 
 #include <cstdio>
+#include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
 #include <hal/Value.h>
+#include <hal/simulation/PCMData.h>
 #include <imgui.h>
-#include <mockdata/PCMData.h>
 
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
 #include "SimDeviceGui.h"
 
 using namespace halsimgui;
 
-static void DisplayCompressors() {
-  static int numPcm = HAL_GetNumPCMModules();
-  for (int i = 0; i < numPcm; ++i) {
-    if (!HALSIM_GetPCMCompressorInitialized(i)) continue;
-    char name[32];
-    std::snprintf(name, sizeof(name), "Compressor[%d]", i);
-    if (SimDeviceGui::StartDevice(name)) {
-      HAL_Value value;
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(PCMCompressorOn, "Compressor On");
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(PCMClosedLoopEnabled, "Closed Loop");
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(PCMPressureSwitch, "Pressure Switch");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PCMCompressorCurrent, "Comp Current");
+struct CompressorSource {
+  explicit CompressorSource(int32_t index)
+      : running{index}, enabled{index}, pressureSwitch{index}, current{index} {}
+  PCMCompressorOnSource running;
+  PCMClosedLoopEnabledSource enabled;
+  PCMPressureSwitchSource pressureSwitch;
+  PCMCompressorCurrentSource current;
+};
+}  // namespace
 
-      // enabled
-      if (HALSimGui::AreOutputsDisabled())
-        value = HAL_MakeBoolean(false);
-      else
-        value = HAL_MakeBoolean(HALSIM_GetPCMCompressorOn(i));
-      if (SimDeviceGui::DisplayValue("Running", false, &value))
-        HALSIM_SetPCMCompressorOn(i, value.data.v_boolean);
+static std::vector<std::unique_ptr<CompressorSource>> gCompressorSources;
 
-      // closed loop
-      value = HAL_MakeEnum(HALSIM_GetPCMClosedLoopEnabled(i) ? 1 : 0);
-      static const char* enabledOptions[] = {"disabled", "enabled"};
-      if (SimDeviceGui::DisplayValue("Closed Loop", true, &value,
-                                     enabledOptions, 2))
-        HALSIM_SetPCMClosedLoopEnabled(i, value.data.v_enum);
-
-      // pressure switch
-      value = HAL_MakeEnum(HALSIM_GetPCMPressureSwitch(i) ? 1 : 0);
-      static const char* switchOptions[] = {"full", "low"};
-      if (SimDeviceGui::DisplayValue("Pressure", false, &value, switchOptions,
-                                     2))
-        HALSIM_SetPCMPressureSwitch(i, value.data.v_enum);
-
-      // compressor current
-      value = HAL_MakeDouble(HALSIM_GetPCMCompressorCurrent(i));
-      if (SimDeviceGui::DisplayValue("Current (A)", false, &value))
-        HALSIM_SetPCMCompressorCurrent(i, value.data.v_double);
-
-      SimDeviceGui::FinishDevice();
+static void UpdateCompressorSources() {
+  for (int i = 0, iend = gCompressorSources.size(); i < iend; ++i) {
+    auto& source = gCompressorSources[i];
+    if (HALSIM_GetPCMCompressorInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<CompressorSource>(i);
+      }
+    } else {
+      source.reset();
     }
   }
 }
 
-void CompressorGui::Initialize() { SimDeviceGui::Add(DisplayCompressors); }
+static void DisplayCompressors() {
+  for (int i = 0, iend = gCompressorSources.size(); i < iend; ++i) {
+    if (auto source = gCompressorSources[i].get()) {
+      char name[32];
+      std::snprintf(name, sizeof(name), "Compressor[%d]", i);
+      if (SimDeviceGui::StartDevice(name)) {
+        HAL_Value value;
+
+        // enabled
+        if (HALSimGui::AreOutputsDisabled())
+          value = HAL_MakeBoolean(false);
+        else
+          value = HAL_MakeBoolean(source->running.GetValue());
+        if (SimDeviceGui::DisplayValueSource("Running", false, &value,
+                                             &source->running))
+          HALSIM_SetPCMCompressorOn(i, value.data.v_boolean);
+
+        // closed loop
+        value = HAL_MakeEnum(source->enabled.GetValue() ? 1 : 0);
+        static const char* enabledOptions[] = {"disabled", "enabled"};
+        if (SimDeviceGui::DisplayValueSource("Closed Loop", true, &value,
+                                             &source->enabled, enabledOptions,
+                                             2))
+          HALSIM_SetPCMClosedLoopEnabled(i, value.data.v_enum);
+
+        // pressure switch
+        value = HAL_MakeEnum(source->pressureSwitch.GetValue() ? 1 : 0);
+        static const char* switchOptions[] = {"full", "low"};
+        if (SimDeviceGui::DisplayValueSource("Pressure", false, &value,
+                                             &source->pressureSwitch,
+                                             switchOptions, 2))
+          HALSIM_SetPCMPressureSwitch(i, value.data.v_enum);
+
+        // compressor current
+        value = HAL_MakeDouble(source->current.GetValue());
+        if (SimDeviceGui::DisplayValueSource("Current (A)", false, &value,
+                                             &source->current))
+          HALSIM_SetPCMCompressorCurrent(i, value.data.v_double);
+
+        SimDeviceGui::FinishDevice();
+      }
+    }
+  }
+}
+
+void CompressorGui::Initialize() {
+  gCompressorSources.resize(HAL_GetNumPCMModules());
+  HALSimGui::AddExecute(UpdateCompressorSources);
+  SimDeviceGui::Add(DisplayCompressors);
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
index 7c9eae5..2f82ff2 100644
--- a/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,34 +7,97 @@
 
 #include "DIOGui.h"
 
-#include <cstdio>
-#include <cstring>
 #include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/DIOData.h>
+#include <hal/simulation/DigitalPWMData.h>
+#include <hal/simulation/DutyCycleData.h>
+#include <hal/simulation/EncoderData.h>
+#include <hal/simulation/SimDeviceData.h>
 #include <imgui.h>
-#include <mockdata/DIOData.h>
-#include <mockdata/DigitalPWMData.h>
-#include <mockdata/DutyCycleData.h>
-#include <mockdata/EncoderData.h>
-#include <mockdata/SimDeviceData.h>
 
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(DIOValue, "DIO");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(DigitalPWMDutyCycle, "DPWM");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(DutyCycleOutput, "DutyCycle");
+}  // namespace
+
+static IniSaver<NameInfo> gDIO{"DIO"};
+static std::vector<std::unique_ptr<DIOValueSource>> gDIOSources;
+static std::vector<std::unique_ptr<DigitalPWMDutyCycleSource>> gDPWMSources;
+static std::vector<std::unique_ptr<DutyCycleOutputSource>> gDutyCycleSources;
+
 static void LabelSimDevice(const char* name, HAL_SimDeviceHandle simDevice) {
   ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
   ImGui::LabelText(name, "%s", HALSIM_GetSimDeviceName(simDevice));
   ImGui::PopStyleColor();
 }
 
+static void UpdateDIOSources() {
+  for (int i = 0, iend = gDIOSources.size(); i < iend; ++i) {
+    auto& source = gDIOSources[i];
+    if (HALSIM_GetDIOInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<DIOValueSource>(i);
+        source->SetName(gDIO[i].GetName());
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
+static void UpdateDPWMSources() {
+  const int numDIO = gDIOSources.size();
+  for (int i = 0, iend = gDPWMSources.size(); i < iend; ++i) {
+    auto& source = gDPWMSources[i];
+    if (HALSIM_GetDigitalPWMInitialized(i)) {
+      if (!source) {
+        int channel = HALSIM_GetDigitalPWMPin(i);
+        if (channel >= 0 && channel < numDIO) {
+          source = std::make_unique<DigitalPWMDutyCycleSource>(i, channel);
+          source->SetName(gDIO[channel].GetName());
+        }
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
+static void UpdateDutyCycleSources() {
+  const int numDIO = gDIOSources.size();
+  for (int i = 0, iend = gDutyCycleSources.size(); i < iend; ++i) {
+    auto& source = gDutyCycleSources[i];
+    if (HALSIM_GetDutyCycleInitialized(i)) {
+      if (!source) {
+        int channel = HALSIM_GetDutyCycleDigitalChannel(i);
+        if (channel >= 0 && channel < numDIO) {
+          source = std::make_unique<DutyCycleOutputSource>(i, channel);
+          source->SetName(gDIO[channel].GetName());
+        }
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
 static void DisplayDIO() {
   bool hasAny = false;
-  static int numDIO = HAL_GetNumDigitalChannels();
-  static int numPWM = HAL_GetNumDigitalPWMOutputs();
-  static int numEncoder = HAL_GetNumEncoders();
-  static int numDutyCycle = HAL_GetNumDutyCycles();
+  const int numDIO = gDIOSources.size();
+  const int numPWM = gDPWMSources.size();
+  static const int numEncoder = HAL_GetNumEncoders();
+  const int numDutyCycle = gDutyCycleSources.size();
   static auto pwmMap = std::make_unique<int[]>(numDIO);
   static auto encoderMap = std::make_unique<int[]>(numDIO);
   static auto dutyCycleMap = std::make_unique<int[]>(numDIO);
@@ -44,8 +107,8 @@
   std::memset(dutyCycleMap.get(), 0, numDIO * sizeof(dutyCycleMap[0]));
 
   for (int i = 0; i < numPWM; ++i) {
-    if (HALSIM_GetDigitalPWMInitialized(i)) {
-      int channel = HALSIM_GetDigitalPWMPin(i);
+    if (auto source = gDPWMSources[i].get()) {
+      int channel = source->GetChannel();
       if (channel >= 0 && channel < numDIO) pwmMap[channel] = i + 1;
     }
   }
@@ -61,64 +124,76 @@
   }
 
   for (int i = 0; i < numDutyCycle; ++i) {
-    if (HALSIM_GetDutyCycleInitialized(i)) {
-      int channel = HALSIM_GetDutyCycleDigitalChannel(i);
+    if (auto source = gDutyCycleSources[i].get()) {
+      int channel = source->GetChannel();
       if (channel >= 0 && channel < numDIO) dutyCycleMap[channel] = i + 1;
     }
   }
 
   ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
   for (int i = 0; i < numDIO; ++i) {
-    if (HALSIM_GetDIOInitialized(i)) {
+    if (auto dioSource = gDIOSources[i].get()) {
+      ImGui::PushID(i);
       hasAny = true;
-      char name[32];
+      DigitalPWMDutyCycleSource* dpwmSource = nullptr;
+      DutyCycleOutputSource* dutyCycleSource = nullptr;
+      auto& info = gDIO[i];
+      char label[128];
       if (pwmMap[i] > 0) {
-        std::snprintf(name, sizeof(name), "PWM[%d]", i);
+        dpwmSource = gDPWMSources[pwmMap[i] - 1].get();
+        info.GetLabel(label, sizeof(label), "PWM", i);
         if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
-          LabelSimDevice(name, simDevice);
+          LabelSimDevice(label, simDevice);
         } else {
-          ImGui::LabelText(name, "%0.3f",
-                           HALSIM_GetDigitalPWMDutyCycle(pwmMap[i] - 1));
+          dpwmSource->LabelText(label, "%0.3f", dpwmSource->GetValue());
         }
       } else if (encoderMap[i] > 0) {
-        std::snprintf(name, sizeof(name), " In[%d]", i);
+        info.GetLabel(label, sizeof(label), " In", i);
         if (auto simDevice = HALSIM_GetEncoderSimDevice(encoderMap[i] - 1)) {
-          LabelSimDevice(name, simDevice);
+          LabelSimDevice(label, simDevice);
         } else {
           ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-          ImGui::LabelText(name, "Encoder[%d,%d]",
+          ImGui::LabelText(label, "Encoder[%d,%d]",
                            HALSIM_GetEncoderDigitalChannelA(encoderMap[i] - 1),
                            HALSIM_GetEncoderDigitalChannelB(encoderMap[i] - 1));
           ImGui::PopStyleColor();
         }
       } else if (dutyCycleMap[i] > 0) {
-        std::snprintf(name, sizeof(name), "PWM[%d]", i);
+        dutyCycleSource = gDutyCycleSources[dutyCycleMap[i] - 1].get();
+        info.GetLabel(label, sizeof(label), "Dty", i);
         if (auto simDevice =
                 HALSIM_GetDutyCycleSimDevice(dutyCycleMap[i] - 1)) {
-          LabelSimDevice(name, simDevice);
+          LabelSimDevice(label, simDevice);
         } else {
-          double val = HALSIM_GetDutyCycleOutput(dutyCycleMap[i] - 1);
-          if (ImGui::InputDouble(name, &val))
+          double val = dutyCycleSource->GetValue();
+          if (dutyCycleSource->InputDouble(label, &val))
             HALSIM_SetDutyCycleOutput(dutyCycleMap[i] - 1, val);
         }
       } else if (!HALSIM_GetDIOIsInput(i)) {
-        std::snprintf(name, sizeof(name), "Out[%d]", i);
+        info.GetLabel(label, sizeof(label), "Out", i);
         if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
-          LabelSimDevice(name, simDevice);
+          LabelSimDevice(label, simDevice);
         } else {
-          ImGui::LabelText(name, "%s",
-                           HALSIM_GetDIOValue(i) ? "1 (high)" : "0 (low)");
+          dioSource->LabelText(
+              label, "%s", dioSource->GetValue() != 0 ? "1 (high)" : "0 (low)");
         }
       } else {
-        std::snprintf(name, sizeof(name), " In[%d]", i);
+        info.GetLabel(label, sizeof(label), " In", i);
         if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
-          LabelSimDevice(name, simDevice);
+          LabelSimDevice(label, simDevice);
         } else {
           static const char* options[] = {"0 (low)", "1 (high)"};
-          int val = HALSIM_GetDIOValue(i) ? 1 : 0;
-          if (ImGui::Combo(name, &val, options, 2)) HALSIM_SetDIOValue(i, val);
+          int val = dioSource->GetValue() != 0 ? 1 : 0;
+          if (dioSource->Combo(label, &val, options, 2))
+            HALSIM_SetDIOValue(i, val);
         }
       }
+      if (info.PopupEditName(i)) {
+        dioSource->SetName(info.GetName());
+        if (dpwmSource) dpwmSource->SetName(info.GetName());
+        if (dutyCycleSource) dutyCycleSource->SetName(info.GetName());
+      }
+      ImGui::PopID();
     }
   }
   ImGui::PopItemWidth();
@@ -126,6 +201,14 @@
 }
 
 void DIOGui::Initialize() {
+  gDIO.Initialize();
+  gDIOSources.resize(HAL_GetNumDigitalChannels());
+  gDPWMSources.resize(HAL_GetNumDigitalPWMOutputs());
+  gDutyCycleSources.resize(HAL_GetNumDutyCycles());
+
+  HALSimGui::AddExecute(UpdateDIOSources);
+  HALSimGui::AddExecute(UpdateDPWMSources);
+  HALSimGui::AddExecute(UpdateDutyCycleSources);
   HALSimGui::AddWindow("DIO", DisplayDIO, ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("DIO", 470, 20);
 }

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
index 1e2f879..96b53cb 100644
--- a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,21 +7,26 @@
 
 #include "DriverStationGui.h"
 
+#include <atomic>
 #include <cstring>
+#include <memory>
 #include <string>
+#include <vector>
 
 #include <GLFW/glfw3.h>
+#include <hal/simulation/DriverStationData.h>
+#include <hal/simulation/MockHooks.h>
 #include <imgui.h>
 #include <imgui_internal.h>
-#include <mockdata/DriverStationData.h>
-#include <mockdata/MockHooks.h>
 #include <wpi/Format.h>
 #include <wpi/SmallString.h>
 #include <wpi/StringRef.h>
 #include <wpi/raw_ostream.h>
 
 #include "ExtraGuiWidgets.h"
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
@@ -45,6 +50,7 @@
 };
 
 struct RobotJoystick {
+  NameInfo name;
   std::string guid;
   const SystemJoystick* sys = nullptr;
   bool useGamepad = false;
@@ -54,11 +60,38 @@
   HAL_JoystickButtons buttons;
   HAL_JoystickPOVs povs;
 
+  void Clear();
   void Update();
   void SetHAL(int i);
+  void GetHAL(int i);
   bool IsButtonPressed(int i) { return (buttons.buttons & (1u << i)) != 0; }
 };
 
+class JoystickSource {
+ public:
+  explicit JoystickSource(int index);
+  ~JoystickSource() {
+    HALSIM_CancelDriverStationNewDataCallback(m_callback);
+    for (int i = 0; i < buttonCount; ++i) delete buttons[i];
+  }
+  JoystickSource(const JoystickSource&) = delete;
+  JoystickSource& operator=(const JoystickSource&) = delete;
+
+  int axisCount;
+  int buttonCount;
+  int povCount;
+  std::unique_ptr<GuiDataSource> axes[HAL_kMaxJoystickAxes];
+  // use pointer instead of unique_ptr to allow it to be passed directly
+  // to DrawLEDSources()
+  GuiDataSource* buttons[32];
+  std::unique_ptr<GuiDataSource> povs[HAL_kMaxJoystickPOVs];
+
+ private:
+  static void CallbackFunc(const char*, void* param, const HAL_Value*);
+
+  int m_index;
+  int32_t m_callback;
+};
 }  // namespace
 
 // system joysticks
@@ -67,8 +100,71 @@
 
 // robot joysticks
 static RobotJoystick gRobotJoysticks[HAL_kMaxJoysticks];
+static std::unique_ptr<JoystickSource> gJoystickSources[HAL_kMaxJoysticks];
 
 static bool gDisableDS = false;
+static std::atomic<bool>* gDSSocketConnected = nullptr;
+
+static inline bool IsDSDisabled() {
+  return gDisableDS || (gDSSocketConnected && *gDSSocketConnected);
+}
+
+JoystickSource::JoystickSource(int index) : m_index{index} {
+  HAL_JoystickAxes halAxes;
+  HALSIM_GetJoystickAxes(index, &halAxes);
+  axisCount = halAxes.count;
+  for (int i = 0; i < axisCount; ++i) {
+    axes[i] = std::make_unique<GuiDataSource>("Joystick[" + wpi::Twine{index} +
+                                              "] Axis[" + wpi::Twine{i} +
+                                              wpi::Twine{']'});
+  }
+
+  HAL_JoystickButtons halButtons;
+  HALSIM_GetJoystickButtons(index, &halButtons);
+  buttonCount = halButtons.count;
+  for (int i = 0; i < buttonCount; ++i) {
+    buttons[i] =
+        new GuiDataSource("Joystick[" + wpi::Twine{index} + "] Button[" +
+                          wpi::Twine{i + 1} + wpi::Twine{']'});
+    buttons[i]->SetDigital(true);
+  }
+  for (int i = buttonCount; i < 32; ++i) buttons[i] = nullptr;
+
+  HAL_JoystickPOVs halPOVs;
+  HALSIM_GetJoystickPOVs(index, &halPOVs);
+  povCount = halPOVs.count;
+  for (int i = 0; i < povCount; ++i) {
+    povs[i] = std::make_unique<GuiDataSource>("Joystick[" + wpi::Twine{index} +
+                                              "] POV[" + wpi::Twine{i} +
+                                              wpi::Twine{']'});
+  }
+
+  m_callback =
+      HALSIM_RegisterDriverStationNewDataCallback(CallbackFunc, this, true);
+}
+
+void JoystickSource::CallbackFunc(const char*, void* param, const HAL_Value*) {
+  auto self = static_cast<JoystickSource*>(param);
+
+  HAL_JoystickAxes halAxes;
+  HALSIM_GetJoystickAxes(self->m_index, &halAxes);
+  for (int i = 0; i < halAxes.count; ++i) {
+    if (auto axis = self->axes[i].get()) axis->SetValue(halAxes.axes[i]);
+  }
+
+  HAL_JoystickButtons halButtons;
+  HALSIM_GetJoystickButtons(self->m_index, &halButtons);
+  for (int i = 0; i < halButtons.count; ++i) {
+    if (auto button = self->buttons[i])
+      button->SetValue((halButtons.buttons & (1u << i)) != 0 ? 1 : 0);
+  }
+
+  HAL_JoystickPOVs halPOVs;
+  HALSIM_GetJoystickPOVs(self->m_index, &halPOVs);
+  for (int i = 0; i < halPOVs.count; ++i) {
+    if (auto pov = self->povs[i].get()) pov->SetValue(halPOVs.povs[i]);
+  }
+}
 
 // read/write joystick mapping to ini file
 static void* JoystickReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
@@ -93,6 +189,8 @@
     int num;
     if (value.getAsInteger(10, num)) return;
     joy->useGamepad = num;
+  } else {
+    joy->name.ReadIni(name, value);
   }
 }
 
@@ -100,11 +198,15 @@
                              ImGuiTextBuffer* out_buf) {
   for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
     auto& joy = gRobotJoysticks[i];
-    if (!joy.sys) continue;
-    const char* guid = glfwGetJoystickGUID(joy.sys - gSystemJoysticks);
-    if (!guid) continue;
-    out_buf->appendf("[Joystick][%d]\nguid=%s\nuseGamepad=%d\n\n", i, guid,
+    if (!joy.name.HasName() && !joy.sys) continue;
+    out_buf->appendf("[Joystick][%d]\nuseGamepad=%d\n", i,
                      joy.useGamepad ? 1 : 0);
+    if (joy.name.HasName()) joy.name.WriteIni(out_buf);
+    if (joy.sys) {
+      const char* guid = glfwGetJoystickGUID(joy.sys - gSystemJoysticks);
+      if (guid) out_buf->appendf("guid=%s\n", guid);
+    }
+    out_buf->append("\n");
   }
 }
 
@@ -198,12 +300,16 @@
   }
 }
 
-void RobotJoystick::Update() {
+void RobotJoystick::Clear() {
   std::memset(&desc, 0, sizeof(desc));
   desc.type = -1;
   std::memset(&axes, 0, sizeof(axes));
   std::memset(&buttons, 0, sizeof(buttons));
   std::memset(&povs, 0, sizeof(povs));
+}
+
+void RobotJoystick::Update() {
+  Clear();
 
   if (!sys || !sys->present) return;
 
@@ -271,19 +377,40 @@
   HALSIM_SetJoystickPOVs(i, &povs);
 }
 
+void RobotJoystick::GetHAL(int i) {
+  HALSIM_GetJoystickDescriptor(i, &desc);
+  HALSIM_GetJoystickAxes(i, &axes);
+  HALSIM_GetJoystickButtons(i, &buttons);
+  HALSIM_GetJoystickPOVs(i, &povs);
+}
+
 static void DriverStationExecute() {
-  static bool prevDisableDS = false;
-  if (gDisableDS && !prevDisableDS) {
-    HALSimGui::SetWindowVisibility("FMS", HALSimGui::kDisabled);
-    HALSimGui::SetWindowVisibility("System Joysticks", HALSimGui::kDisabled);
-    HALSimGui::SetWindowVisibility("Joysticks", HALSimGui::kDisabled);
-  } else if (!gDisableDS && prevDisableDS) {
-    HALSimGui::SetWindowVisibility("FMS", HALSimGui::kShow);
-    HALSimGui::SetWindowVisibility("System Joysticks", HALSimGui::kShow);
-    HALSimGui::SetWindowVisibility("Joysticks", HALSimGui::kShow);
+  // update sources
+  for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
+    auto& source = gJoystickSources[i];
+    int32_t axisCount, buttonCount, povCount;
+    HALSIM_GetJoystickCounts(i, &axisCount, &buttonCount, &povCount);
+    if (axisCount != 0 || buttonCount != 0 || povCount != 0) {
+      if (!source || source->axisCount != axisCount ||
+          source->buttonCount != buttonCount || source->povCount != povCount) {
+        source.reset();
+        source = std::make_unique<JoystickSource>(i);
+      }
+    } else {
+      source.reset();
+    }
   }
-  prevDisableDS = gDisableDS;
-  if (gDisableDS) return;
+
+  static bool prevDisableDS = false;
+
+  bool disableDS = IsDSDisabled();
+  if (disableDS && !prevDisableDS) {
+    HALSimGui::SetWindowVisibility("System Joysticks", HALSimGui::kDisabled);
+  } else if (!disableDS && prevDisableDS) {
+    HALSimGui::SetWindowVisibility("System Joysticks", HALSimGui::kShow);
+  }
+  prevDisableDS = disableDS;
+  if (disableDS) return;
 
   double curTime = glfwGetTime();
 
@@ -336,22 +463,44 @@
 }
 
 static void DisplayFMS() {
+  bool fmsAttached = HALSIM_GetDriverStationFmsAttached();
+  bool dsAttached = HALSIM_GetDriverStationDsAttached();
+  static const char* stations[] = {"Red 1",  "Red 2",  "Red 3",
+                                   "Blue 1", "Blue 2", "Blue 3"};
+  int allianceStationId = HALSIM_GetDriverStationAllianceStationId();
+  double matchTime = HALSIM_GetDriverStationMatchTime();
+  HAL_MatchInfo matchInfo;
+  HALSIM_GetMatchInfo(&matchInfo);
+
+  if (IsDSDisabled()) {
+    if (!HALSIM_GetDriverStationEnabled())
+      ImGui::Text("Robot State: Disabled");
+    else if (HALSIM_GetDriverStationTest())
+      ImGui::Text("Robot State: Test");
+    else if (HALSIM_GetDriverStationAutonomous())
+      ImGui::Text("Robot State: Autonomous");
+    else
+      ImGui::Text("Robot State: Teleoperated");
+
+    ImGui::Text("FMS Attached: %s", fmsAttached ? "Yes" : "No");
+    ImGui::Text("DS Attached: %s", dsAttached ? "Yes" : "No");
+    ImGui::Text("Alliance Station: %s", stations[allianceStationId]);
+    ImGui::Text("Match Time: %.1f", matchTime);
+    ImGui::Text("Game Specific: %s", matchInfo.gameSpecificMessage);
+    return;
+  }
+
   double curTime = glfwGetTime();
 
   // FMS Attached
-  bool fmsAttached = HALSIM_GetDriverStationFmsAttached();
   if (ImGui::Checkbox("FMS Attached", &fmsAttached))
     HALSIM_SetDriverStationFmsAttached(fmsAttached);
 
   // DS Attached
-  bool dsAttached = HALSIM_GetDriverStationDsAttached();
   if (ImGui::Checkbox("DS Attached", &dsAttached))
     HALSIM_SetDriverStationDsAttached(dsAttached);
 
   // Alliance Station
-  static const char* stations[] = {"Red 1",  "Red 2",  "Red 3",
-                                   "Blue 1", "Blue 2", "Blue 3"};
-  int allianceStationId = HALSIM_GetDriverStationAllianceStationId();
   ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
   if (ImGui::Combo("Alliance Station", &allianceStationId, stations, 6))
     HALSIM_SetDriverStationAllianceStationId(
@@ -362,7 +511,6 @@
   ImGui::Checkbox("Match Time Enabled", &matchTimeEnabled);
 
   static double startMatchTime = 0.0;
-  double matchTime = HALSIM_GetDriverStationMatchTime();
   ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
   if (ImGui::InputDouble("Match Time", &matchTime, 0, 0, "%.1f",
                          ImGuiInputTextFlags_EnterReturnsTrue)) {
@@ -380,7 +528,6 @@
   }
 
   // Game Specific Message
-  static HAL_MatchInfo matchInfo;
   ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
   if (ImGui::InputText("Game Specific",
                        reinterpret_cast<char*>(matchInfo.gameSpecificMessage),
@@ -420,15 +567,16 @@
 }
 
 static void DisplayJoysticks() {
+  bool disableDS = IsDSDisabled();
   // imgui doesn't size columns properly with autoresize, so force it
   ImGui::Dummy(ImVec2(ImGui::GetFontSize() * 10 * HAL_kMaxJoysticks, 0));
 
   ImGui::Columns(HAL_kMaxJoysticks, "Joysticks", false);
   for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
     auto& joy = gRobotJoysticks[i];
-    char label[30];
-    std::snprintf(label, sizeof(label), "Joystick %d", i);
-    if (joy.sys) {
+    char label[128];
+    joy.name.GetLabel(label, sizeof(label), "Joystick", i);
+    if (!disableDS && joy.sys) {
       ImGui::Selectable(label, false);
       if (ImGui::BeginDragDropSource()) {
         ImGui::SetDragDropPayload("Joystick", &joy.sys, sizeof(joy.sys));
@@ -439,7 +587,7 @@
     } else {
       ImGui::Selectable(label, false, ImGuiSelectableFlags_Disabled);
     }
-    if (ImGui::BeginDragDropTarget()) {
+    if (!disableDS && ImGui::BeginDragDropTarget()) {
       if (const ImGuiPayload* payload =
               ImGui::AcceptDragDropPayload("Joystick")) {
         IM_ASSERT(payload->DataSize == sizeof(SystemJoystick*));
@@ -455,26 +603,55 @@
       }
       ImGui::EndDragDropTarget();
     }
+    joy.name.PopupEditName(i);
     ImGui::NextColumn();
   }
   ImGui::Separator();
 
   for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
     auto& joy = gRobotJoysticks[i];
+    auto source = gJoystickSources[i].get();
 
-    if (joy.sys && joy.sys->present) {
+    if (disableDS) joy.GetHAL(i);
+
+    if ((disableDS && joy.desc.type != 0) || (joy.sys && joy.sys->present)) {
       // update GUI display
       ImGui::PushID(i);
-      ImGui::Text("%d: %s", static_cast<int>(joy.sys - gSystemJoysticks),
-                  joy.sys->name);
+      if (disableDS) {
+        ImGui::Text("%s", joy.desc.name);
+        ImGui::Text("Gamepad: %s", joy.desc.isXbox ? "Yes" : "No");
+      } else {
+        ImGui::Text("%d: %s", static_cast<int>(joy.sys - gSystemJoysticks),
+                    joy.sys->name);
 
-      if (joy.sys->isGamepad) ImGui::Checkbox("Map gamepad", &joy.useGamepad);
+        if (joy.sys->isGamepad) ImGui::Checkbox("Map gamepad", &joy.useGamepad);
+      }
 
-      for (int j = 0; j < joy.axes.count; ++j)
-        ImGui::Text("Axis[%d]: %.3f", j, joy.axes.axes[j]);
+      for (int j = 0; j < joy.axes.count; ++j) {
+        if (source && source->axes[j]) {
+          char label[64];
+          std::snprintf(label, sizeof(label), "Axis[%d]", j);
+          ImGui::Selectable(label);
+          source->axes[j]->EmitDrag();
+          ImGui::SameLine();
+          ImGui::Text(": %.3f", joy.axes.axes[j]);
+        } else {
+          ImGui::Text("Axis[%d]: %.3f", j, joy.axes.axes[j]);
+        }
+      }
 
-      for (int j = 0; j < joy.povs.count; ++j)
-        ImGui::Text("POVs[%d]: %d", j, joy.povs.povs[j]);
+      for (int j = 0; j < joy.povs.count; ++j) {
+        if (source && source->povs[j]) {
+          char label[64];
+          std::snprintf(label, sizeof(label), "POVs[%d]", j);
+          ImGui::Selectable(label);
+          source->povs[j]->EmitDrag();
+          ImGui::SameLine();
+          ImGui::Text(": %d", joy.povs.povs[j]);
+        } else {
+          ImGui::Text("POVs[%d]: %d", j, joy.povs.povs[j]);
+        }
+      }
 
       // show buttons as multiple lines of LED indicators, 8 per line
       static const ImU32 color = IM_COL32(255, 255, 102, 255);
@@ -482,7 +659,8 @@
       buttons.resize(joy.buttons.count);
       for (int j = 0; j < joy.buttons.count; ++j)
         buttons[j] = joy.IsButtonPressed(j) ? 1 : -1;
-      DrawLEDs(buttons.data(), buttons.size(), 8, &color);
+      DrawLEDSources(buttons.data(), source ? source->buttons : nullptr,
+                     buttons.size(), 8, &color);
       ImGui::PopID();
     } else {
       ImGui::Text("Unassigned");
@@ -493,7 +671,11 @@
 }
 
 static void DriverStationOptionMenu() {
-  ImGui::MenuItem("Turn off DS", nullptr, &gDisableDS);
+  if (gDSSocketConnected && *gDSSocketConnected) {
+    ImGui::MenuItem("Turn off DS (real DS connected)", nullptr, true, false);
+  } else {
+    ImGui::MenuItem("Turn off DS", nullptr, &gDisableDS);
+  }
 }
 
 void DriverStationGui::Initialize() {
@@ -526,3 +708,7 @@
   HALSimGui::SetDefaultWindowPos("System Joysticks", 5, 385);
   HALSimGui::SetDefaultWindowPos("Joysticks", 250, 465);
 }
+
+void DriverStationGui::SetDSSocketExtension(void* data) {
+  gDSSocketConnected = static_cast<std::atomic<bool>*>(data);
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h
index 571c265..5bf25b5 100644
--- a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h
+++ b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 class DriverStationGui {
  public:
   static void Initialize();
+  static void SetDSSocketExtension(void* data);
 };
 
 }  // namespace halsimgui

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/EncoderGui.cpp b/simulation/halsim_gui/src/main/native/cpp/EncoderGui.cpp
index 55650fb..5b169f0 100644
--- a/simulation/halsim_gui/src/main/native/cpp/EncoderGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/EncoderGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,105 +7,268 @@
 
 #include "EncoderGui.h"
 
-#include <cstdio>
+#include <limits>
+#include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/EncoderData.h>
+#include <hal/simulation/SimDeviceData.h>
 #include <imgui.h>
-#include <imgui_internal.h>
-#include <mockdata/EncoderData.h>
-#include <mockdata/SimDeviceData.h>
-#include <wpi/DenseMap.h>
-#include <wpi/StringRef.h>
 
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
-static wpi::DenseMap<int, bool> gEncodersOpen;  // indexed by channel A
+namespace {
 
-// read/write open state to ini file
-static void* EncodersReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                              const char* name) {
-  int num;
-  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
-  return &gEncodersOpen[num];
-}
-
-static void EncodersReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             void* entry, const char* lineStr) {
-  bool* element = static_cast<bool*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  if (name == "open") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    *element = num;
+struct EncoderInfo : public NameInfo, public OpenInfo {
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
+    if (NameInfo::ReadIni(name, value)) return true;
+    if (OpenInfo::ReadIni(name, value)) return true;
+    return false;
   }
-}
+  void WriteIni(ImGuiTextBuffer* out) {
+    NameInfo::WriteIni(out);
+    OpenInfo::WriteIni(out);
+  }
+};
 
-static void EncodersWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             ImGuiTextBuffer* out_buf) {
-  for (auto it : gEncodersOpen)
-    out_buf->appendf("[Encoder][%d]\nopen=%d\n\n", it.first, it.second ? 1 : 0);
+class EncoderSource {
+ public:
+  EncoderSource(const wpi::Twine& id, int32_t index, int channelA, int channelB)
+      : distancePerPulse(id + " Dist/Count"),
+        count(id + " Count"),
+        period(id + " Period"),
+        direction(id + " Direction"),
+        distance(id + " Distance"),
+        rate(id + " Rate"),
+        m_index{index},
+        m_channelA{channelA},
+        m_channelB{channelB},
+        m_distancePerPulseCallback{
+            HALSIM_RegisterEncoderDistancePerPulseCallback(
+                index, DistancePerPulseCallbackFunc, this, true)},
+        m_countCallback{HALSIM_RegisterEncoderCountCallback(
+            index, CountCallbackFunc, this, true)},
+        m_periodCallback{HALSIM_RegisterEncoderPeriodCallback(
+            index, PeriodCallbackFunc, this, true)},
+        m_directionCallback{HALSIM_RegisterEncoderDirectionCallback(
+            index, DirectionCallbackFunc, this, true)} {
+    direction.SetDigital(true);
+  }
+
+  EncoderSource(int32_t index, int channelA, int channelB)
+      : EncoderSource("Encoder[" + wpi::Twine(channelA) + wpi::Twine(',') +
+                          wpi::Twine(channelB) + wpi::Twine(']'),
+                      index, channelA, channelB) {}
+
+  explicit EncoderSource(int32_t index)
+      : EncoderSource(index, HALSIM_GetEncoderDigitalChannelA(index),
+                      HALSIM_GetEncoderDigitalChannelB(index)) {}
+
+  ~EncoderSource() {
+    if (m_distancePerPulseCallback != 0)
+      HALSIM_CancelEncoderDistancePerPulseCallback(m_index,
+                                                   m_distancePerPulseCallback);
+    if (m_countCallback != 0)
+      HALSIM_CancelEncoderCountCallback(m_index, m_countCallback);
+    if (m_periodCallback != 0)
+      HALSIM_CancelEncoderCountCallback(m_index, m_periodCallback);
+    if (m_directionCallback != 0)
+      HALSIM_CancelEncoderCountCallback(m_index, m_directionCallback);
+  }
+
+  void SetName(const wpi::Twine& name) {
+    if (name.str().empty()) {
+      distancePerPulse.SetName("");
+      count.SetName("");
+      period.SetName("");
+      direction.SetName("");
+      distance.SetName("");
+      rate.SetName("");
+    } else {
+      distancePerPulse.SetName(name + " Distance/Count");
+      count.SetName(name + " Count");
+      period.SetName(name + " Period");
+      direction.SetName(name + " Direction");
+      distance.SetName(name + " Distance");
+      rate.SetName(name + " Rate");
+    }
+  }
+
+  int32_t GetIndex() const { return m_index; }
+  int GetChannelA() const { return m_channelA; }
+  int GetChannelB() const { return m_channelB; }
+
+  GuiDataSource distancePerPulse;
+  GuiDataSource count;
+  GuiDataSource period;
+  GuiDataSource direction;
+  GuiDataSource distance;
+  GuiDataSource rate;
+
+ private:
+  static void DistancePerPulseCallbackFunc(const char*, void* param,
+                                           const HAL_Value* value) {
+    if (value->type == HAL_DOUBLE) {
+      auto self = static_cast<EncoderSource*>(param);
+      double distPerPulse = value->data.v_double;
+      self->distancePerPulse.SetValue(distPerPulse);
+      self->distance.SetValue(self->count.GetValue() * distPerPulse);
+      double period = self->period.GetValue();
+      if (period == 0)
+        self->rate.SetValue(std::numeric_limits<double>::infinity());
+      else if (period == std::numeric_limits<double>::infinity())
+        self->rate.SetValue(0);
+      else
+        self->rate.SetValue(static_cast<float>(distPerPulse / period));
+    }
+  }
+
+  static void CountCallbackFunc(const char*, void* param,
+                                const HAL_Value* value) {
+    if (value->type == HAL_INT) {
+      auto self = static_cast<EncoderSource*>(param);
+      double count = value->data.v_int;
+      self->count.SetValue(count);
+      self->distance.SetValue(count * self->distancePerPulse.GetValue());
+    }
+  }
+
+  static void PeriodCallbackFunc(const char*, void* param,
+                                 const HAL_Value* value) {
+    if (value->type == HAL_DOUBLE) {
+      auto self = static_cast<EncoderSource*>(param);
+      double period = value->data.v_double;
+      self->period.SetValue(period);
+      if (period == 0)
+        self->rate.SetValue(std::numeric_limits<double>::infinity());
+      else if (period == std::numeric_limits<double>::infinity())
+        self->rate.SetValue(0);
+      else
+        self->rate.SetValue(
+            static_cast<float>(self->distancePerPulse.GetValue() / period));
+    }
+  }
+
+  static void DirectionCallbackFunc(const char*, void* param,
+                                    const HAL_Value* value) {
+    if (value->type == HAL_BOOLEAN) {
+      static_cast<EncoderSource*>(param)->direction.SetValue(
+          value->data.v_boolean);
+    }
+  }
+
+  int32_t m_index;
+  int m_channelA;
+  int m_channelB;
+  int32_t m_distancePerPulseCallback;
+  int32_t m_countCallback;
+  int32_t m_periodCallback;
+  int32_t m_directionCallback;
+};
+
+}  // namespace
+
+static IniSaver<EncoderInfo> gEncoders{"Encoder"};  // indexed by channel A
+static std::vector<std::unique_ptr<EncoderSource>> gEncoderSources;
+
+static void UpdateEncoderSources() {
+  for (int i = 0, iend = gEncoderSources.size(); i < iend; ++i) {
+    auto& source = gEncoderSources[i];
+    if (HALSIM_GetEncoderInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<EncoderSource>(i);
+        source->SetName(gEncoders[source->GetChannelA()].GetName());
+      }
+    } else {
+      source.reset();
+    }
+  }
 }
 
 static void DisplayEncoders() {
   bool hasAny = false;
-  static int numEncoder = HAL_GetNumEncoders();
   ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
-  for (int i = 0; i < numEncoder; ++i) {
-    if (HALSIM_GetEncoderInitialized(i)) {
+  for (int i = 0, iend = gEncoderSources.size(); i < iend; ++i) {
+    if (auto source = gEncoderSources[i].get()) {
       hasAny = true;
-      char name[32];
-      int chA = HALSIM_GetEncoderDigitalChannelA(i);
-      int chB = HALSIM_GetEncoderDigitalChannelB(i);
-      std::snprintf(name, sizeof(name), "Encoder[%d,%d]", chA, chB);
       if (auto simDevice = HALSIM_GetEncoderSimDevice(i)) {
         ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
         ImGui::Text("%s", HALSIM_GetSimDeviceName(simDevice));
         ImGui::PopStyleColor();
-      } else if (ImGui::CollapsingHeader(
-                     name,
-                     gEncodersOpen[chA] ? ImGuiTreeNodeFlags_DefaultOpen : 0)) {
-        gEncodersOpen[chA] = true;
-
-        ImGui::PushID(i);
-
-        // distance per pulse
-        double distancePerPulse = HALSIM_GetEncoderDistancePerPulse(i);
-        ImGui::LabelText("Dist/Count", "%.6f", distancePerPulse);
-
-        // count
-        int count = HALSIM_GetEncoderCount(i);
-        if (ImGui::InputInt("Count", &count)) HALSIM_SetEncoderCount(i, count);
-        ImGui::SameLine();
-        if (ImGui::Button("Reset")) HALSIM_SetEncoderCount(i, 0);
-
-        // max period
-        double maxPeriod = HALSIM_GetEncoderMaxPeriod(i);
-        ImGui::LabelText("Max Period", "%.6f", maxPeriod);
-
-        // period
-        double period = HALSIM_GetEncoderPeriod(i);
-        if (ImGui::InputDouble("Period", &period, 0, 0, "%.6g"))
-          HALSIM_SetEncoderPeriod(i, period);
-
-        // reverse direction
-        ImGui::LabelText(
-            "Reverse Direction", "%s",
-            HALSIM_GetEncoderReverseDirection(i) ? "true" : "false");
-
-        // direction
-        static const char* options[] = {"reverse", "forward"};
-        int direction = HALSIM_GetEncoderDirection(i) ? 1 : 0;
-        if (ImGui::Combo("Direction", &direction, options, 2))
-          HALSIM_SetEncoderDirection(i, direction);
-
-        ImGui::PopID();
       } else {
-        gEncodersOpen[chA] = false;
+        int chA = source->GetChannelA();
+        int chB = source->GetChannelB();
+
+        // build header name
+        auto& info = gEncoders[chA];
+        char name[128];
+        info.GetLabel(name, sizeof(name), "Encoder", chA, chB);
+
+        // header
+        bool open = ImGui::CollapsingHeader(
+            name, gEncoders[chA].IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
+        info.SetOpen(open);
+
+        // context menu to change name
+        if (info.PopupEditName(chA)) {
+          source->SetName(info.GetName());
+        }
+
+        if (open) {
+          ImGui::PushID(i);
+          // distance per pulse
+          double distancePerPulse = source->distancePerPulse.GetValue();
+          source->distancePerPulse.LabelText("Dist/Count", "%.6f",
+                                             distancePerPulse);
+
+          // count
+          int count = source->count.GetValue();
+          if (ImGui::InputInt("##input", &count))
+            HALSIM_SetEncoderCount(i, count);
+          ImGui::SameLine();
+          if (ImGui::Button("Reset")) HALSIM_SetEncoderCount(i, 0);
+          ImGui::SameLine();
+          ImGui::Selectable("Count");
+          source->count.EmitDrag();
+
+          // max period
+          double maxPeriod = HALSIM_GetEncoderMaxPeriod(i);
+          ImGui::LabelText("Max Period", "%.6f", maxPeriod);
+
+          // period
+          double period = source->period.GetValue();
+          if (source->period.InputDouble("Period", &period, 0, 0, "%.6g"))
+            HALSIM_SetEncoderPeriod(i, period);
+
+          // reverse direction
+          ImGui::LabelText(
+              "Reverse Direction", "%s",
+              HALSIM_GetEncoderReverseDirection(i) ? "true" : "false");
+
+          // direction
+          static const char* options[] = {"reverse", "forward"};
+          int direction = source->direction.GetValue() ? 1 : 0;
+          if (source->direction.Combo("Direction", &direction, options, 2))
+            HALSIM_SetEncoderDirection(i, direction);
+
+          // distance
+          double distance = source->distance.GetValue();
+          if (source->distance.InputDouble("Distance", &distance, 0, 0, "%.6g"))
+            HALSIM_SetEncoderDistance(i, distance);
+
+          // rate
+          double rate = source->rate.GetValue();
+          if (source->rate.InputDouble("Rate", &rate, 0, 0, "%.6g"))
+            HALSIM_SetEncoderRate(i, rate);
+
+          ImGui::PopID();
+        }
       }
     }
   }
@@ -114,16 +277,10 @@
 }
 
 void EncoderGui::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "Encoder";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = EncodersReadOpen;
-  iniHandler.ReadLineFn = EncodersReadLine;
-  iniHandler.WriteAllFn = EncodersWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
+  gEncoders.Initialize();
+  gEncoderSources.resize(HAL_GetNumEncoders());
+  HALSimGui::AddExecute(UpdateEncoderSources);
   HALSimGui::AddWindow("Encoders", DisplayEncoders,
                        ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("Encoders", 640, 215);
+  HALSimGui::SetDefaultWindowPos("Encoders", 5, 250);
 }

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h b/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h
index 3e8297b..b7c9dbf 100644
--- a/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h
+++ b/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp b/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
index 3b1ba28..b5bf92f 100644
--- a/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,10 +7,13 @@
 
 #include "ExtraGuiWidgets.h"
 
+#include "GuiDataSource.h"
+
 namespace halsimgui {
 
-void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
-              float size, float spacing, const LEDConfig& config) {
+void DrawLEDSources(const int* values, GuiDataSource** sources, int numValues,
+                    int cols, const ImU32* colors, float size, float spacing,
+                    const LEDConfig& config) {
   if (numValues == 0 || cols < 1) return;
   if (size == 0) size = ImGui::GetFontSize() / 2.0;
   if (spacing == 0) spacing = ImGui::GetFontSize() / 3.0;
@@ -21,35 +24,35 @@
   ImDrawList* drawList = ImGui::GetWindowDrawList();
   const ImVec2 p = ImGui::GetCursorScreenPos();
 
+  float sized2 = size / 2;
   float ystart, yinc;
   if (config.start & 1) {
     // lower
-    ystart = p.y + size / 2 + inc * (rows - 1);
+    ystart = p.y + sized2 + inc * (rows - 1);
     yinc = -inc;
   } else {
     // upper
-    ystart = p.y + size / 2;
+    ystart = p.y + sized2;
     yinc = inc;
   }
 
   float xstart, xinc;
   if (config.start & 2) {
     // right
-    xstart = p.x + size / 2 + inc * (cols - 1);
+    xstart = p.x + sized2 + inc * (cols - 1);
     xinc = -inc;
   } else {
     // left
-    xstart = p.x + size / 2;
+    xstart = p.x + sized2;
     xinc = inc;
   }
 
   float x = xstart, y = ystart;
-  if (config.order == LEDConfig::RowMajor) {
-    // row major
-    int row = 1;
-    for (int i = 0; i < numValues; ++i) {
-      if (i >= (row * cols)) {
-        ++row;
+  int rowcol = 1;  // row for row-major, column for column-major
+  for (int i = 0; i < numValues; ++i) {
+    if (config.order == LEDConfig::RowMajor) {
+      if (i >= (rowcol * cols)) {
+        ++rowcol;
         if (config.serpentine) {
           x -= xinc;
           xinc = -xinc;
@@ -58,20 +61,9 @@
         }
         y += yinc;
       }
-      if (values[i] > 0)
-        drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
-                                colors[values[i] - 1]);
-      else if (values[i] < 0)
-        drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
-                          colors[-values[i] - 1], 0.0f, 0, 1.0);
-      x += xinc;
-    }
-  } else {
-    // column major
-    int col = 1;
-    for (int i = 0; i < numValues; ++i) {
-      if (i >= (col * rows)) {
-        ++col;
+    } else {
+      if (i >= (rowcol * rows)) {
+        ++rowcol;
         if (config.serpentine) {
           y -= yinc;
           yinc = -yinc;
@@ -80,17 +72,38 @@
         }
         x += xinc;
       }
-      if (values[i] > 0)
-        drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
-                                colors[values[i] - 1]);
-      else if (values[i] < 0)
-        drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
-                          colors[-values[i] - 1], 0.0f, 0, 1.0);
+    }
+    if (values[i] > 0)
+      drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
+                              colors[values[i] - 1]);
+    else if (values[i] < 0)
+      drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
+                        colors[-values[i] - 1], 0.0f, 0, 1.0);
+    if (sources) {
+      ImGui::SetCursorScreenPos(ImVec2(x - sized2, y - sized2));
+      if (sources[i]) {
+        ImGui::PushID(i);
+        ImGui::Selectable("", false, 0, ImVec2(inc, inc));
+        sources[i]->EmitDrag();
+        ImGui::PopID();
+      } else {
+        ImGui::Dummy(ImVec2(inc, inc));
+      }
+    }
+    if (config.order == LEDConfig::RowMajor) {
+      x += xinc;
+    } else {
       y += yinc;
     }
   }
 
-  ImGui::Dummy(ImVec2(inc * cols, inc * rows));
+  if (!sources) ImGui::Dummy(ImVec2(inc * cols, inc * rows));
+}
+
+void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
+              float size, float spacing, const LEDConfig& config) {
+  DrawLEDSources(values, nullptr, numValues, cols, colors, size, spacing,
+                 config);
 }
 
 }  // namespace halsimgui

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

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

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/Field2D.h b/simulation/halsim_gui/src/main/native/cpp/Field2D.h
new file mode 100644
index 0000000..52218f3
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/Field2D.h
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+class Field2D {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/GuiDataSource.cpp b/simulation/halsim_gui/src/main/native/cpp/GuiDataSource.cpp
new file mode 100644
index 0000000..df967c1
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/GuiDataSource.cpp
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GuiDataSource.h"
+
+#include <wpi/StringMap.h>
+
+using namespace halsimgui;
+
+wpi::sig::Signal<const char*, GuiDataSource*> GuiDataSource::sourceCreated;
+
+static wpi::StringMap<GuiDataSource*> gSources;
+
+GuiDataSource::GuiDataSource(const wpi::Twine& id) : m_id{id.str()} {
+  gSources.try_emplace(m_id, this);
+  sourceCreated(m_id.c_str(), this);
+}
+
+GuiDataSource::GuiDataSource(const wpi::Twine& id, int index)
+    : GuiDataSource{id + wpi::Twine('[') + wpi::Twine(index) +
+                    wpi::Twine(']')} {}
+
+GuiDataSource::GuiDataSource(const wpi::Twine& id, int index, int index2)
+    : GuiDataSource{id + wpi::Twine('[') + wpi::Twine(index) + wpi::Twine(',') +
+                    wpi::Twine(index2) + wpi::Twine(']')} {}
+
+GuiDataSource::~GuiDataSource() {
+  auto it = gSources.find(m_id);
+  if (it == gSources.end()) return;
+  if (it->getValue() == this) gSources.erase(it);
+}
+
+void GuiDataSource::LabelText(const char* label, const char* fmt, ...) const {
+  va_list args;
+  va_start(args, fmt);
+  LabelTextV(label, fmt, args);
+  va_end(args);
+}
+
+// Add a label+text combo aligned to other label+value widgets
+void GuiDataSource::LabelTextV(const char* label, const char* fmt,
+                               va_list args) const {
+  ImGui::PushID(label);
+  ImGui::LabelTextV("##input", fmt, args);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  ImGui::PopID();
+  EmitDrag();
+}
+
+bool GuiDataSource::Combo(const char* label, int* current_item,
+                          const char* const items[], int items_count,
+                          int popup_max_height_in_items) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::Combo("##input", current_item, items, items_count,
+                         popup_max_height_in_items);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+bool GuiDataSource::SliderFloat(const char* label, float* v, float v_min,
+                                float v_max, const char* format,
+                                float power) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::SliderFloat("##input", v, v_min, v_max, format, power);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+bool GuiDataSource::InputDouble(const char* label, double* v, double step,
+                                double step_fast, const char* format,
+                                ImGuiInputTextFlags flags) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::InputDouble("##input", v, step, step_fast, format, flags);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+bool GuiDataSource::InputInt(const char* label, int* v, int step, int step_fast,
+                             ImGuiInputTextFlags flags) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::InputInt("##input", v, step, step_fast, flags);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+void GuiDataSource::EmitDrag(ImGuiDragDropFlags flags) const {
+  if (ImGui::BeginDragDropSource(flags)) {
+    auto self = this;
+    ImGui::SetDragDropPayload("DataSource", &self, sizeof(self));
+    ImGui::TextUnformatted(m_name.empty() ? m_id.c_str() : m_name.c_str());
+    ImGui::EndDragDropSource();
+  }
+}
+
+GuiDataSource* GuiDataSource::Find(wpi::StringRef id) {
+  auto it = gSources.find(id);
+  if (it == gSources.end()) return nullptr;
+  return it->getValue();
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp b/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp
index 79df3df..9803494 100644
--- a/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,21 +8,18 @@
 #include "HALSimGui.h"
 
 #include <algorithm>
-#include <atomic>
 
-#include <GL/gl3w.h>
-#include <GLFW/glfw3.h>
+#include <hal/simulation/DriverStationData.h>
 #include <imgui.h>
-#include <imgui_ProggyDotted.h>
-#include <imgui_impl_glfw.h>
-#include <imgui_impl_opengl3.h>
 #include <imgui_internal.h>
-#include <mockdata/DriverStationData.h>
 #include <wpi/StringMap.h>
 #include <wpi/raw_ostream.h>
+#include <wpigui.h>
 
 using namespace halsimgui;
 
+namespace gui = wpi::gui;
+
 namespace {
 struct WindowInfo {
   WindowInfo() = default;
@@ -40,57 +37,23 @@
   ImGuiCond sizeCond = 0;
   ImVec2 pos;
   ImVec2 size;
+  bool setPadding = false;
+  ImVec2 padding;
 };
 }  // namespace
 
-static std::atomic_bool gExit{false};
-static GLFWwindow* gWindow;
-static bool gWindowLoadedWidthHeight = false;
-static int gWindowWidth = 1280;
-static int gWindowHeight = 720;
-static int gWindowMaximized = 0;
-static int gWindowXPos = -1;
-static int gWindowYPos = -1;
-static std::vector<std::function<void()>> gInitializers;
-static std::vector<std::function<void()>> gExecutors;
 static std::vector<WindowInfo> gWindows;
 static wpi::StringMap<int> gWindowMap;   // index into gWindows
 static std::vector<int> gSortedWindows;  // index into gWindows
 static std::vector<std::function<void()>> gOptionMenus;
 static std::vector<std::function<void()>> gMenus;
-static int gUserScale = 2;
-static int gStyle = 0;
-static constexpr int kScaledFontLevels = 9;
-static ImFont* gScaledFont[kScaledFontLevels];
 static bool gDisableOutputsOnDSDisable = true;
 
-static void glfw_error_callback(int error, const char* description) {
-  wpi::errs() << "GLFW Error " << error << ": " << description << '\n';
-}
-
-static void glfw_window_size_callback(GLFWwindow*, int width, int height) {
-  if (!gWindowMaximized) {
-    gWindowWidth = width;
-    gWindowHeight = height;
-  }
-}
-
-static void glfw_window_maximize_callback(GLFWwindow* window, int maximized) {
-  gWindowMaximized = maximized;
-}
-
-static void glfw_window_pos_callback(GLFWwindow* window, int xpos, int ypos) {
-  if (!gWindowMaximized) {
-    gWindowXPos = xpos;
-    gWindowYPos = ypos;
-  }
-}
-
 // read/write open state to ini file
 static void* SimWindowsReadOpen(ImGuiContext* ctx,
                                 ImGuiSettingsHandler* handler,
                                 const char* name) {
-  if (wpi::StringRef{name} == "GLOBAL") return &gWindow;
+  if (wpi::StringRef{name} == "GLOBAL") return &gDisableOutputsOnDSDisable;
 
   int index = gWindowMap.try_emplace(name, gWindows.size()).first->second;
   if (index == static_cast<int>(gWindows.size())) {
@@ -109,26 +72,10 @@
   name = name.trim();
   value = value.trim();
 
-  if (entry == &gWindow) {
+  if (entry == &gDisableOutputsOnDSDisable) {
     int num;
     if (value.getAsInteger(10, num)) return;
-    if (name == "width") {
-      gWindowWidth = num;
-      gWindowLoadedWidthHeight = true;
-    } else if (name == "height") {
-      gWindowHeight = num;
-      gWindowLoadedWidthHeight = true;
-    } else if (name == "maximized") {
-      gWindowMaximized = num;
-    } else if (name == "xpos") {
-      gWindowXPos = num;
-    } else if (name == "ypos") {
-      gWindowYPos = num;
-    } else if (name == "userScale") {
-      gUserScale = num;
-    } else if (name == "style") {
-      gStyle = num;
-    } else if (name == "disableOutputsOnDS") {
+    if (name == "disableOutputsOnDS") {
       gDisableOutputsOnDSDisable = num;
     }
     return;
@@ -148,37 +95,20 @@
 
 static void SimWindowsWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
                                ImGuiTextBuffer* out_buf) {
-  out_buf->appendf(
-      "[SimWindow][GLOBAL]\nwidth=%d\nheight=%d\nmaximized=%d\n"
-      "xpos=%d\nypos=%d\nuserScale=%d\nstyle=%d\ndisableOutputsOnDS=%d\n",
-      gWindowWidth, gWindowHeight, gWindowMaximized, gWindowXPos, gWindowYPos,
-      gUserScale, gStyle, gDisableOutputsOnDSDisable ? 1 : 0);
+  out_buf->appendf("[SimWindow][GLOBAL]\ndisableOutputsOnDS=%d\n\n",
+                   gDisableOutputsOnDSDisable ? 1 : 0);
   for (auto&& window : gWindows)
     out_buf->appendf("[SimWindow][%s]\nvisible=%d\nenabled=%d\n\n",
                      window.name.c_str(), window.visible ? 1 : 0,
                      window.enabled ? 1 : 0);
 }
 
-static void UpdateStyle() {
-  switch (gStyle) {
-    case 0:
-      ImGui::StyleColorsClassic();
-      break;
-    case 1:
-      ImGui::StyleColorsDark();
-      break;
-    case 2:
-      ImGui::StyleColorsLight();
-      break;
-  }
-}
-
 void HALSimGui::Add(std::function<void()> initialize) {
-  if (initialize) gInitializers.emplace_back(std::move(initialize));
+  gui::AddInit(std::move(initialize));
 }
 
 void HALSimGui::AddExecute(std::function<void()> execute) {
-  if (execute) gExecutors.emplace_back(std::move(execute));
+  gui::AddEarlyExecute(std::move(execute));
 }
 
 void HALSimGui::AddWindow(const char* name, std::function<void()> display,
@@ -248,194 +178,45 @@
   window.size = ImVec2{width, height};
 }
 
+void HALSimGui::SetWindowPadding(const char* name, float x, float y) {
+  auto it = gWindowMap.find(name);
+  if (it == gWindowMap.end()) return;
+  auto& window = gWindows[it->second];
+  window.setPadding = true;
+  window.padding = ImVec2{x, y};
+}
+
 bool HALSimGui::AreOutputsDisabled() {
   return gDisableOutputsOnDSDisable && !HALSIM_GetDriverStationEnabled();
 }
 
+void HALSimGui::GlobalInit() { gui::CreateContext(); }
+
 bool HALSimGui::Initialize() {
-  // Setup window
-  glfwSetErrorCallback(glfw_error_callback);
-  glfwInitHint(GLFW_JOYSTICK_HAT_BUTTONS, GLFW_FALSE);
-  if (!glfwInit()) return false;
+  gui::AddInit([] {
+    // Hook ini handler to save settings
+    ImGuiSettingsHandler iniHandler;
+    iniHandler.TypeName = "SimWindow";
+    iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+    iniHandler.ReadOpenFn = SimWindowsReadOpen;
+    iniHandler.ReadLineFn = SimWindowsReadLine;
+    iniHandler.WriteAllFn = SimWindowsWriteAll;
+    ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+  });
 
-    // Decide GL+GLSL versions
-#if __APPLE__
-  // GL 3.2 + GLSL 150
-  const char* glsl_version = "#version 150";
-  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
-  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);
-  glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);  // 3.2+ only
-  glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);  // Required on Mac
-  glfwWindowHint(GLFW_COCOA_GRAPHICS_SWITCHING, GLFW_TRUE);
-#else
-  // GL 3.0 + GLSL 130
-  const char* glsl_version = "#version 130";
-  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
-  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
-  // glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);  // 3.2+
-  // glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); // 3.0+
-#endif
-
-  // Setup Dear ImGui context
-  IMGUI_CHECKVERSION();
-  ImGui::CreateContext();
-  ImGuiIO& io = ImGui::GetIO();
-  (void)io;
-
-  // Hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "SimWindow";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = SimWindowsReadOpen;
-  iniHandler.ReadLineFn = SimWindowsReadLine;
-  iniHandler.WriteAllFn = SimWindowsWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
-  for (auto&& initialize : gInitializers) {
-    if (initialize) initialize();
-  }
-
-  // Load INI file
-  ImGui::LoadIniSettingsFromDisk(io.IniFilename);
-
-  // Set initial window settings
-  glfwWindowHint(GLFW_MAXIMIZED, gWindowMaximized ? GLFW_TRUE : GLFW_FALSE);
-
-  float windowScale = 1.0;
-  if (!gWindowLoadedWidthHeight) {
-    glfwWindowHint(GLFW_SCALE_TO_MONITOR, GLFW_TRUE);
-    // get the primary monitor work area to see if we have a reasonable initial
-    // window size; if not, maximize, and default scaling to smaller
-    if (GLFWmonitor* primary = glfwGetPrimaryMonitor()) {
-      int monWidth, monHeight;
-      glfwGetMonitorWorkarea(primary, nullptr, nullptr, &monWidth, &monHeight);
-      if (monWidth < gWindowWidth || monHeight < gWindowHeight) {
-        glfwWindowHint(GLFW_MAXIMIZED, GLFW_TRUE);
-        windowScale = (std::min)(monWidth * 1.0 / gWindowWidth,
-                                 monHeight * 1.0 / gWindowHeight);
+  gui::AddWindowScaler([](float windowScale) {
+    // scale default window positions
+    for (auto&& window : gWindows) {
+      if ((window.posCond & ImGuiCond_FirstUseEver) != 0) {
+        window.pos.x *= windowScale;
+        window.pos.y *= windowScale;
+        window.size.x *= windowScale;
+        window.size.y *= windowScale;
       }
     }
-  }
-  if (gWindowXPos != -1 && gWindowYPos != -1)
-    glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
+  });
 
-  // Create window with graphics context
-  gWindow = glfwCreateWindow(gWindowWidth, gWindowHeight,
-                             "Robot Simulation GUI", nullptr, nullptr);
-  if (!gWindow) return false;
-
-  if (!gWindowLoadedWidthHeight) {
-    if (windowScale == 1.0)
-      glfwGetWindowContentScale(gWindow, &windowScale, nullptr);
-    wpi::outs() << "windowScale = " << windowScale;
-    // force user scale if window scale is smaller
-    if (windowScale <= 0.5)
-      gUserScale = 0;
-    else if (windowScale <= 0.75)
-      gUserScale = 1;
-    if (windowScale != 1.0) {
-      // scale default window positions
-      for (auto&& window : gWindows) {
-        if ((window.posCond & ImGuiCond_FirstUseEver) != 0) {
-          window.pos.x *= windowScale;
-          window.pos.y *= windowScale;
-        }
-      }
-    }
-  }
-
-  // Update window settings
-  if (gWindowXPos != -1 && gWindowYPos != -1) {
-    glfwSetWindowPos(gWindow, gWindowXPos, gWindowYPos);
-    glfwShowWindow(gWindow);
-  }
-
-  // Set window callbacks
-  glfwGetWindowSize(gWindow, &gWindowWidth, &gWindowHeight);
-  glfwSetWindowSizeCallback(gWindow, glfw_window_size_callback);
-  glfwSetWindowMaximizeCallback(gWindow, glfw_window_maximize_callback);
-  glfwSetWindowPosCallback(gWindow, glfw_window_pos_callback);
-
-  glfwMakeContextCurrent(gWindow);
-  glfwSwapInterval(1);  // Enable vsync
-
-  // Initialize OpenGL loader
-  if (gl3wInit() != 0) {
-    wpi::errs() << "Failed to initialize OpenGL loader!\n";
-    return false;
-  }
-
-  // Setup Dear ImGui style
-  UpdateStyle();
-
-  // Setup Platform/Renderer bindings
-  ImGui_ImplGlfw_InitForOpenGL(gWindow, true);
-  ImGui_ImplOpenGL3_Init(glsl_version);
-
-  // Load Fonts
-  // - If no fonts are loaded, dear imgui will use the default font. You can
-  // also load multiple fonts and use ImGui::PushFont()/PopFont() to select
-  // them.
-  // - AddFontFromFileTTF() will return the ImFont* so you can store it if you
-  // need to select the font among multiple.
-  // - If the file cannot be loaded, the function will return NULL. Please
-  // handle those errors in your application (e.g. use an assertion, or display
-  // an error and quit).
-  // - The fonts will be rasterized at a given size (w/ oversampling) and stored
-  // into a texture when calling ImFontAtlas::Build()/GetTexDataAsXXXX(), which
-  // ImGui_ImplXXXX_NewFrame below will call.
-  // - Read 'misc/fonts/README.txt' for more instructions and details.
-  // - Remember that in C/C++ if you want to include a backslash \ in a string
-  // literal you need to write a double backslash \\ !
-  // io.Fonts->AddFontDefault();
-  // io.Fonts->AddFontFromFileTTF("../../misc/fonts/Roboto-Medium.ttf", 16.0f);
-  // io.Fonts->AddFontFromFileTTF("../../misc/fonts/Cousine-Regular.ttf", 15.0f);
-  // io.Fonts->AddFontFromFileTTF("../../misc/fonts/DroidSans.ttf", 16.0f);
-  // io.Fonts->AddFontFromFileTTF("../../misc/fonts/ProggyTiny.ttf", 10.0f);
-  // ImFont* font =
-  // io.Fonts->AddFontFromFileTTF("c:\\Windows\\Fonts\\ArialUni.ttf", 18.0f,
-  // NULL, io.Fonts->GetGlyphRangesJapanese()); IM_ASSERT(font != NULL);
-
-  // this range is based on 13px being the "nominal" 100% size and going from
-  // ~0.5x (7px) to ~2.0x (25px)
-  for (int i = 0; i < kScaledFontLevels; ++i) {
-    float size = 7.0f + i * 3.0f;
-    ImFontConfig cfg;
-    std::snprintf(cfg.Name, sizeof(cfg.Name), "ProggyDotted-%d",
-                  static_cast<int>(size));
-    gScaledFont[i] = ImGui::AddFontProggyDotted(io, size, &cfg);
-  }
-
-  return true;
-}
-
-void HALSimGui::Main(void*) {
-  ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f);
-
-  // Main loop
-  while (!glfwWindowShouldClose(gWindow) && !gExit) {
-    // Poll and handle events (inputs, window resize, etc.)
-    glfwPollEvents();
-
-    // Start the Dear ImGui frame
-    ImGui_ImplOpenGL3_NewFrame();
-    ImGui_ImplGlfw_NewFrame();
-    ImGui::NewFrame();
-
-    // Scale based on OS window content scaling
-    float windowScale = 1.0;
-    glfwGetWindowContentScale(gWindow, &windowScale, nullptr);
-    // map to closest font size: 0 = 0.5x, 1 = 0.75x, 2 = 1.0x, 3 = 1.25x,
-    // 4 = 1.5x, 5 = 1.75x, 6 = 2x
-    int fontScale =
-        std::clamp(gUserScale + static_cast<int>((windowScale - 1.0) * 4), 0,
-                   kScaledFontLevels - 1);
-    ImGui::GetIO().FontDefault = gScaledFont[fontScale];
-
-    for (auto&& execute : gExecutors) {
-      if (execute) execute();
-    }
-
+  gui::AddLateExecute([] {
     {
       ImGui::BeginMainMenuBar();
 
@@ -448,41 +229,7 @@
         ImGui::EndMenu();
       }
 
-      if (ImGui::BeginMenu("View")) {
-        if (ImGui::BeginMenu("Style")) {
-          bool selected;
-          selected = gStyle == 0;
-          if (ImGui::MenuItem("Classic", nullptr, &selected, true)) {
-            gStyle = 0;
-            UpdateStyle();
-          }
-          selected = gStyle == 1;
-          if (ImGui::MenuItem("Dark", nullptr, &selected, true)) {
-            gStyle = 1;
-            UpdateStyle();
-          }
-          selected = gStyle == 2;
-          if (ImGui::MenuItem("Light", nullptr, &selected, true)) {
-            gStyle = 2;
-            UpdateStyle();
-          }
-          ImGui::EndMenu();
-        }
-
-        if (ImGui::BeginMenu("Zoom")) {
-          for (int i = 0; i < kScaledFontLevels && (25 * (i + 2)) <= 200; ++i) {
-            char label[20];
-            std::snprintf(label, sizeof(label), "%d%%", 25 * (i + 2));
-            bool selected = gUserScale == i;
-            bool enabled = (fontScale - gUserScale + i) >= 0 &&
-                           (fontScale - gUserScale + i) < kScaledFontLevels;
-            if (ImGui::MenuItem(label, nullptr, &selected, enabled))
-              gUserScale = i;
-          }
-          ImGui::EndMenu();
-        }
-        ImGui::EndMenu();
-      }
+      gui::EmitViewMenu();
 
       if (ImGui::BeginMenu("Window")) {
         for (auto&& windowIndex : gSortedWindows) {
@@ -515,34 +262,27 @@
           ImGui::SetNextWindowPos(window.pos, window.posCond);
         if (window.sizeCond != 0)
           ImGui::SetNextWindowSize(window.size, window.sizeCond);
+        if (window.setPadding)
+          ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, window.padding);
         if (ImGui::Begin(window.name.c_str(), &window.visible, window.flags))
           window.display();
         ImGui::End();
+        if (window.setPadding) ImGui::PopStyleVar();
       }
     }
+  });
 
-    // Rendering
-    ImGui::Render();
-    int display_w, display_h;
-    glfwGetFramebufferSize(gWindow, &display_w, &display_h);
-    glViewport(0, 0, display_w, display_h);
-    glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w);
-    glClear(GL_COLOR_BUFFER_BIT);
-    ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());
+  if (!gui::Initialize("", 1280, 720)) return false;
 
-    glfwSwapBuffers(gWindow);
-  }
-
-  // Cleanup
-  ImGui_ImplOpenGL3_Shutdown();
-  ImGui_ImplGlfw_Shutdown();
-  ImGui::DestroyContext();
-
-  glfwDestroyWindow(gWindow);
-  glfwTerminate();
+  return true;
 }
 
-void HALSimGui::Exit(void*) { gExit = true; }
+void HALSimGui::Main(void*) {
+  gui::Main();
+  gui::DestroyContext();
+}
+
+void HALSimGui::Exit(void*) { gui::Exit(); }
 
 extern "C" {
 
@@ -561,7 +301,8 @@
 void HALSIMGUI_AddWindow(const char* name, void* param, void (*display)(void*),
                          int32_t flags) {
   if (display) {
-    HALSimGui::AddWindow(name, [=] { display(param); }, flags);
+    HALSimGui::AddWindow(
+        name, [=] { display(param); }, flags);
   }
 }
 
@@ -591,6 +332,10 @@
   HALSimGui::SetDefaultWindowSize(name, width, height);
 }
 
+void HALSIMGUI_SetWindowPadding(const char* name, float x, float y) {
+  HALSimGui::SetDefaultWindowSize(name, x, y);
+}
+
 int HALSIMGUI_AreOutputsDisabled(void) {
   return HALSimGui::AreOutputsDisabled();
 }

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

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

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp b/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp
new file mode 100644
index 0000000..8203325
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp
@@ -0,0 +1,322 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Mechanism2D.h"
+
+#include <cmath>
+#include <string>
+
+#include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
+#include <imgui.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui_internal.h>
+#include <wpi/json.h>
+#include <wpi/math>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+#include "HALSimGui.h"
+#include "portable-file-dialogs.h"
+
+using namespace halsimgui;
+
+static HAL_SimDeviceHandle devHandle = 0;
+static wpi::StringMap<ImColor> colorLookUpTable;
+static std::unique_ptr<pfd::open_file> m_fileOpener;
+static std::string previousJsonLocation = "Not empty";
+namespace {
+struct BodyConfig {
+  std::string name;
+  std::string type = "line";
+  int length = 100;
+  std::string color = "green";
+  int angle = 0;
+  std::vector<BodyConfig> children;
+  int lineWidth = 1;
+};
+}  // namespace
+static std::vector<BodyConfig> bodyConfigVector;
+namespace {
+struct DrawLineStruct {
+  float xEnd;
+  float yEnd;
+  float angle;
+};
+}  // namespace
+static struct NamedColor {
+  const char* name;
+  ImColor value;
+} staticColors[] = {{"white", IM_COL32(255, 255, 255, 255)},
+                    {"silver", IM_COL32(192, 192, 192, 255)},
+                    {"gray", IM_COL32(128, 128, 128, 255)},
+                    {"black", IM_COL32(0, 0, 0, 255)},
+                    {"red", IM_COL32(255, 0, 0, 255)},
+                    {"maroon", IM_COL32(128, 0, 0, 255)},
+                    {"yellow", IM_COL32(255, 255, 0, 255)},
+                    {"olive", IM_COL32(128, 128, 0, 255)},
+                    {"lime", IM_COL32(0, 255, 0, 255)},
+                    {"green", IM_COL32(0, 128, 0, 255)},
+                    {"aqua", IM_COL32(0, 255, 255, 255)},
+                    {"teal", IM_COL32(0, 128, 128, 255)},
+                    {"blue", IM_COL32(0, 0, 255, 255)},
+                    {"navy", IM_COL32(0, 0, 128, 255)},
+                    {"fuchsia", IM_COL32(255, 0, 255, 255)},
+                    {"purple", IM_COL32(128, 0, 128, 255)}};
+
+static void buildColorTable() {
+  for (auto&& namedColor : staticColors) {
+    colorLookUpTable.try_emplace(namedColor.name, namedColor.value);
+  }
+}
+namespace {
+class Mechanism2DInfo {
+ public:
+  std::string jsonLocation;
+};
+}  // namespace
+
+static Mechanism2DInfo mechanism2DInfo;
+
+bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
+  if (name == "jsonLocation") {
+    mechanism2DInfo.jsonLocation = value;
+  } else {
+    return false;
+  }
+  return true;
+}
+
+void WriteIni(ImGuiTextBuffer* out) {
+  out->appendf("[Mechanism2D][Mechanism2D]\njsonLocation=%s\n\n",
+               mechanism2DInfo.jsonLocation.c_str());
+}
+
+// read/write settings to ini file
+static void* Mechanism2DReadOpen(ImGuiContext* ctx,
+                                 ImGuiSettingsHandler* handler,
+                                 const char* name) {
+  if (name == wpi::StringRef{"Mechanism2D"}) return &mechanism2DInfo;
+  return nullptr;
+}
+
+static void Mechanism2DReadLine(ImGuiContext* ctx,
+                                ImGuiSettingsHandler* handler, void* entry,
+                                const char* lineStr) {
+  wpi::StringRef line{lineStr};
+  auto [name, value] = line.split('=');
+  name = name.trim();
+  value = value.trim();
+  if (entry == &mechanism2DInfo) ReadIni(name, value);
+}
+
+static void Mechanism2DWriteAll(ImGuiContext* ctx,
+                                ImGuiSettingsHandler* handler,
+                                ImGuiTextBuffer* out_buf) {
+  WriteIni(out_buf);
+}
+
+static void GetJsonFileLocation() {
+  if (m_fileOpener && m_fileOpener->ready(0)) {
+    auto result = m_fileOpener->result();
+    if (!result.empty()) {
+      mechanism2DInfo.jsonLocation = result[0];
+    } else {
+      wpi::errs() << "Can not find json file!!!";
+    }
+  }
+}
+
+DrawLineStruct DrawLine(float startXLocation, float startYLocation, int length,
+                        float angle, ImDrawList* drawList, ImVec2 windowPos,
+                        ImColor color, const BodyConfig& bodyConfig,
+                        const std::string& previousPath) {
+  DrawLineStruct drawLineStruct;
+  drawLineStruct.angle = angle;
+  // Find the current path do the ligament
+  std::string currentPath = previousPath + bodyConfig.name;
+  // Find the angle in radians
+  double radAngle = (drawLineStruct.angle - 90) * wpi::math::pi / 180;
+  // Get the start X and Y location
+  drawLineStruct.xEnd = startXLocation + length * std::cos(radAngle);
+  drawLineStruct.yEnd = startYLocation + length * std::sin(radAngle);
+  // Add the line to the drawList
+  drawList->AddLine(
+      windowPos + ImVec2(startXLocation, startYLocation),
+      windowPos + ImVec2(drawLineStruct.xEnd, drawLineStruct.yEnd), color,
+      bodyConfig.lineWidth);
+  // Return the end X, Y, and angle
+  return drawLineStruct;
+}
+
+static void buildDrawList(float startXLocation, float startYLocation,
+                          ImDrawList* drawList, float previousAngle,
+                          const std::vector<BodyConfig>& subBodyConfigs,
+                          ImVec2 windowPos) {
+  for (BodyConfig const& bodyConfig : subBodyConfigs) {
+    hal::SimDouble angleHandle;
+    hal::SimDouble lengthHandle;
+    float angle = 0;
+    float length = 0;
+    // Get the smallest of width or height
+    double minSize;
+    // Find the min size of the window
+    minSize = ImGui::GetWindowHeight() > ImGui::GetWindowWidth()
+                  ? ImGui::GetWindowWidth()
+                  : ImGui::GetWindowHeight();
+    if (devHandle == 0) devHandle = HALSIM_GetSimDeviceHandle("Mechanism2D");
+    // Get the length
+    if (!lengthHandle)
+      lengthHandle = HALSIM_GetSimValueHandle(
+          devHandle, (bodyConfig.name + "/length").c_str());
+    if (lengthHandle) length = lengthHandle.Get();
+    if (length <= 0) {
+      length = bodyConfig.length;
+    }
+    // Get the angle
+    if (!angleHandle)
+      angleHandle = HALSIM_GetSimValueHandle(
+          devHandle, (bodyConfig.name + "/angle").c_str());
+    if (angleHandle) angle = angleHandle.Get();
+    // Calculate the next angle to go to
+    float angleToGoTo = angle + bodyConfig.angle + previousAngle;
+    // Draw the first line and get the ending coordinates
+
+    DrawLineStruct drawLine =
+        DrawLine(startXLocation, startYLocation, minSize / 100 * length,
+                 angleToGoTo, drawList, windowPos,
+                 colorLookUpTable[bodyConfig.color], bodyConfig, "");
+
+    // If the line has children then draw them with the stating points being the
+    // end of the parent
+    if (!bodyConfig.children.empty()) {
+      buildDrawList(drawLine.xEnd, drawLine.yEnd, drawList, drawLine.angle,
+                    bodyConfig.children, windowPos);
+    }
+  }
+}
+
+static BodyConfig readSubJson(const std::string& name, wpi::json const& body) {
+  BodyConfig c;
+  try {
+    c.name = name + "/" + body.at("name").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "could not read body name: " << e.what() << '\n';
+  }
+  try {
+    c.length = body.at("length").get<int>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "length '" << c.name
+                << "': could not find length path: " << e.what() << '\n';
+  }
+  try {
+    c.color = body.at("color").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "color '" << c.name
+                << "': could not find color path: " << e.what() << '\n';
+  }
+  try {
+    c.angle = body.at("angle").get<int>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "angle '" << c.name
+                << "': could not find angle path: " << e.what() << '\n';
+  }
+  try {
+    c.lineWidth = body.at("lineWidth").get<int>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "lineWidth '" << c.name
+                << "': could not find lineWidth path: " << e.what() << '\n';
+  }
+  try {
+    for (wpi::json const& child : body.at("children")) {
+      c.children.push_back(readSubJson(c.name, child));
+      wpi::outs() << "Reading Child with name " << c.name << '\n';
+    }
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "could not read body: " << e.what() << '\n';
+  }
+  return c;
+}
+
+static void readJson(std::string jFile) {
+  std::error_code ec;
+  std::string name;
+  wpi::raw_fd_istream is(jFile, ec);
+  if (ec) {
+    wpi::errs() << "could not open '" << jFile << "': " << ec.message() << '\n';
+  }
+  // parse file
+  wpi::json j;
+  try {
+    j = wpi::json::parse(is);
+  } catch (const wpi::json::parse_error& e) {
+    wpi::errs() << "byte " << e.byte << ": " << e.what() << '\n';
+  }
+  // top level must be an object
+  if (!j.is_object()) {
+    wpi::errs() << "must be JSON object\n";
+  }
+  try {
+    name = j.at("name").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "name '" << name
+                << "': could not find name path: " << e.what() << '\n';
+  }
+  try {
+    for (wpi::json const& body : j.at("body")) {
+      bodyConfigVector.push_back(readSubJson(name, body));
+    }
+  } catch (const wpi::json::exception& e) {
+    wpi::errs() << "could not read body: " << e.what() << '\n';
+  }
+}
+
+static void OptionMenuLocateJson() {
+  if (ImGui::BeginMenu("Mechanism2D")) {
+    if (ImGui::MenuItem("Load Json")) {
+      m_fileOpener = std::make_unique<pfd::open_file>(
+          "Choose Mechanism2D json", "", std::vector<std::string>{"*.json"});
+    }
+    ImGui::EndMenu();
+  }
+}
+
+static void DisplayAssembly2D() {
+  GetJsonFileLocation();
+  if (!mechanism2DInfo.jsonLocation.empty()) {
+    // Only read the json file if it changed
+    if (mechanism2DInfo.jsonLocation != previousJsonLocation) {
+      bodyConfigVector.clear();
+      readJson(mechanism2DInfo.jsonLocation);
+    }
+    previousJsonLocation = mechanism2DInfo.jsonLocation;
+    ImVec2 windowPos = ImGui::GetWindowPos();
+    ImDrawList* drawList = ImGui::GetWindowDrawList();
+    buildDrawList(ImGui::GetWindowWidth() / 2, ImGui::GetWindowHeight(),
+                  drawList, 0, bodyConfigVector, windowPos);
+  }
+}
+
+void Mechanism2D::Initialize() {
+  // hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = "Mechanism2D";
+  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+  iniHandler.ReadOpenFn = Mechanism2DReadOpen;
+  iniHandler.ReadLineFn = Mechanism2DReadLine;
+  iniHandler.WriteAllFn = Mechanism2DWriteAll;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+
+  buildColorTable();
+  HALSimGui::AddWindow("Mechanism 2D", DisplayAssembly2D);
+  HALSimGui::SetWindowVisibility("Mechanism 2D", HALSimGui::kHide);
+  HALSimGui::AddOptionMenu(OptionMenuLocateJson);
+  HALSimGui::SetDefaultWindowPos("Mechanism 2D", 200, 200);
+  HALSimGui::SetDefaultWindowSize("Mechanism 2D", 600, 600);
+  HALSimGui::SetWindowPadding("Mechanism 2D", 0, 0);
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h b/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h
new file mode 100644
index 0000000..2f291dd
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+class Mechanism2D {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.cpp b/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.cpp
new file mode 100644
index 0000000..2d442b5
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.cpp
@@ -0,0 +1,409 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "NetworkTablesGui.h"
+
+#include <cstdio>
+#include <cstring>
+#include <initializer_list>
+#include <memory>
+#include <vector>
+
+#include <imgui.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/NetworkTableValue.h>
+#include <ntcore_cpp.h>
+#include <wpi/DenseMap.h>
+#include <wpi/Format.h>
+#include <wpi/SmallString.h>
+#include <wpi/StringRef.h>
+#include <wpi/raw_ostream.h>
+
+#include "GuiDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+static NT_EntryListenerPoller gNetworkTablesPoller;
+static wpi::DenseMap<NT_Entry, std::unique_ptr<GuiDataSource>>
+    gNetworkTableSources;
+
+static void UpdateNetworkTableSources() {
+  bool timedOut = false;
+  for (auto&& event :
+       nt::PollEntryListener(gNetworkTablesPoller, 0, &timedOut)) {
+    if (!event.value->IsBoolean() && !event.value->IsDouble()) continue;
+    if (event.flags & NT_NOTIFY_NEW) {
+      auto& source = gNetworkTableSources[event.entry];
+      if (!source)
+        source =
+            std::make_unique<GuiDataSource>(wpi::Twine{"NT:"} + event.name);
+    }
+    if (event.flags & NT_NOTIFY_DELETE) {
+      if (auto& source = gNetworkTableSources[event.entry]) source.reset();
+    }
+    if (event.flags & (NT_NOTIFY_NEW | NT_NOTIFY_UPDATE)) {
+      if (auto& source = gNetworkTableSources[event.entry]) {
+        if (event.value->IsBoolean()) {
+          source->SetValue(event.value->GetBoolean() ? 1 : 0);
+          source->SetDigital(true);
+        } else if (event.value->IsDouble()) {
+          source->SetValue(event.value->GetDouble());
+          source->SetDigital(false);
+        }
+      }
+    }
+  }
+}
+
+static void BooleanArrayToString(wpi::SmallVectorImpl<char>& out,
+                                 wpi::ArrayRef<int> in) {
+  out.clear();
+  wpi::raw_svector_ostream os{out};
+  os << '[';
+  bool first = true;
+  for (auto v : in) {
+    if (!first) os << ',';
+    first = false;
+    if (v)
+      os << "true";
+    else
+      os << "false";
+  }
+  os << ']';
+}
+
+static std::shared_ptr<nt::Value> StringToBooleanArray(wpi::StringRef in) {
+  in = in.trim();
+  if (in.empty())
+    return nt::NetworkTableValue::MakeBooleanArray(
+        std::initializer_list<bool>{});
+  if (in.front() == '[') in = in.drop_front();
+  if (in.back() == ']') in = in.drop_back();
+  in = in.trim();
+
+  wpi::SmallVector<wpi::StringRef, 16> inSplit;
+  wpi::SmallVector<int, 16> out;
+
+  in.split(inSplit, ',', -1, false);
+  for (auto val : inSplit) {
+    val = val.trim();
+    if (val.equals_lower("true")) {
+      out.emplace_back(1);
+    } else if (val.equals_lower("false")) {
+      out.emplace_back(0);
+    } else {
+      wpi::errs() << "GUI: NetworkTables: Could not understand value '" << val
+                  << "'\n";
+      return nullptr;
+    }
+  }
+
+  return nt::NetworkTableValue::MakeBooleanArray(out);
+}
+
+static void DoubleArrayToString(wpi::SmallVectorImpl<char>& out,
+                                wpi::ArrayRef<double> in) {
+  out.clear();
+  wpi::raw_svector_ostream os{out};
+  os << '[';
+  bool first = true;
+  for (auto v : in) {
+    if (!first) os << ',';
+    first = false;
+    os << wpi::format("%.6f", v);
+  }
+  os << ']';
+}
+
+static std::shared_ptr<nt::Value> StringToDoubleArray(wpi::StringRef in) {
+  in = in.trim();
+  if (in.empty())
+    return nt::NetworkTableValue::MakeBooleanArray(
+        std::initializer_list<bool>{});
+  if (in.front() == '[') in = in.drop_front();
+  if (in.back() == ']') in = in.drop_back();
+  in = in.trim();
+
+  wpi::SmallVector<wpi::StringRef, 16> inSplit;
+  wpi::SmallVector<double, 16> out;
+
+  in.split(inSplit, ',', -1, false);
+  for (auto val : inSplit) {
+    val = val.trim();
+    wpi::SmallString<32> valStr = val;
+    double d;
+    if (std::sscanf(valStr.c_str(), "%lf", &d) == 1) {
+      out.emplace_back(d);
+    } else {
+      wpi::errs() << "GUI: NetworkTables: Could not understand value '" << val
+                  << "'\n";
+      return nullptr;
+    }
+  }
+
+  return nt::NetworkTableValue::MakeDoubleArray(out);
+}
+
+static void StringArrayToString(wpi::SmallVectorImpl<char>& out,
+                                wpi::ArrayRef<std::string> in) {
+  out.clear();
+  wpi::raw_svector_ostream os{out};
+  os << '[';
+  bool first = true;
+  for (auto&& v : in) {
+    if (!first) os << ',';
+    first = false;
+    os << '"';
+    os.write_escaped(v);
+    os << '"';
+  }
+  os << ']';
+}
+
+static int fromxdigit(char ch) {
+  if (ch >= 'a' && ch <= 'f')
+    return (ch - 'a' + 10);
+  else if (ch >= 'A' && ch <= 'F')
+    return (ch - 'A' + 10);
+  else
+    return ch - '0';
+}
+
+static wpi::StringRef UnescapeString(wpi::StringRef source,
+                                     wpi::SmallVectorImpl<char>& buf) {
+  assert(source.size() >= 2 && source.front() == '"' && source.back() == '"');
+  buf.clear();
+  buf.reserve(source.size() - 2);
+  for (auto s = source.begin() + 1, end = source.end() - 1; s != end; ++s) {
+    if (*s != '\\') {
+      buf.push_back(*s);
+      continue;
+    }
+    switch (*++s) {
+      case 't':
+        buf.push_back('\t');
+        break;
+      case 'n':
+        buf.push_back('\n');
+        break;
+      case 'x': {
+        if (!isxdigit(*(s + 1))) {
+          buf.push_back('x');  // treat it like a unknown escape
+          break;
+        }
+        int ch = fromxdigit(*++s);
+        if (std::isxdigit(*(s + 1))) {
+          ch <<= 4;
+          ch |= fromxdigit(*++s);
+        }
+        buf.push_back(static_cast<char>(ch));
+        break;
+      }
+      default:
+        buf.push_back(*s);
+        break;
+    }
+  }
+  return wpi::StringRef{buf.data(), buf.size()};
+}
+
+static std::shared_ptr<nt::Value> StringToStringArray(wpi::StringRef in) {
+  in = in.trim();
+  if (in.empty())
+    return nt::NetworkTableValue::MakeStringArray(
+        std::initializer_list<std::string>{});
+  if (in.front() == '[') in = in.drop_front();
+  if (in.back() == ']') in = in.drop_back();
+  in = in.trim();
+
+  wpi::SmallVector<wpi::StringRef, 16> inSplit;
+  std::vector<std::string> out;
+  wpi::SmallString<32> buf;
+
+  in.split(inSplit, ',', -1, false);
+  for (auto val : inSplit) {
+    val = val.trim();
+    if (val.empty()) continue;
+    if (val.front() != '"' || val.back() != '"') {
+      wpi::errs() << "GUI: NetworkTables: Could not understand value '" << val
+                  << "'\n";
+      return nullptr;
+    }
+    out.emplace_back(UnescapeString(val, buf));
+  }
+
+  return nt::NetworkTableValue::MakeStringArray(std::move(out));
+}
+
+static constexpr size_t kTextBufferSize = 4096;
+
+static char* GetTextBuffer(wpi::StringRef in) {
+  static char textBuffer[kTextBufferSize];
+  size_t len = (std::min)(in.size(), kTextBufferSize - 1);
+  std::memcpy(textBuffer, in.data(), len);
+  textBuffer[len] = '\0';
+  return textBuffer;
+}
+
+static void DisplayNetworkTables() {
+  static auto inst = nt::NetworkTableInstance::GetDefault();
+
+  if (ImGui::CollapsingHeader("Connections")) {
+    ImGui::Columns(4, "connections");
+    ImGui::Text("Id");
+    ImGui::NextColumn();
+    ImGui::Text("Address");
+    ImGui::NextColumn();
+    ImGui::Text("Updated");
+    ImGui::NextColumn();
+    ImGui::Text("Proto");
+    ImGui::NextColumn();
+    ImGui::Separator();
+    for (auto&& i : inst.GetConnections()) {
+      ImGui::Text("%s", i.remote_id.c_str());
+      ImGui::NextColumn();
+      ImGui::Text("%s", i.remote_ip.c_str());
+      ImGui::NextColumn();
+      ImGui::Text("%llu",
+                  static_cast<unsigned long long>(  // NOLINT(runtime/int)
+                      i.last_update));
+      ImGui::NextColumn();
+      ImGui::Text("%d.%d", i.protocol_version >> 8, i.protocol_version & 0xff);
+      ImGui::NextColumn();
+    }
+    ImGui::Columns();
+  }
+
+  if (ImGui::CollapsingHeader("Values", ImGuiTreeNodeFlags_DefaultOpen)) {
+    static bool first = true;
+    ImGui::Columns(4, "values");
+    if (first) ImGui::SetColumnWidth(-1, 0.5f * ImGui::GetWindowWidth());
+    ImGui::Text("Name");
+    ImGui::NextColumn();
+    ImGui::Text("Value");
+    ImGui::NextColumn();
+    if (first) ImGui::SetColumnWidth(-1, 12 * ImGui::GetFontSize());
+    ImGui::Text("Flags");
+    ImGui::NextColumn();
+    ImGui::Text("Changed");
+    ImGui::NextColumn();
+    ImGui::Separator();
+    first = false;
+
+    auto info = inst.GetEntryInfo("", 0);
+    std::sort(info.begin(), info.end(),
+              [](const auto& a, const auto& b) { return a.name < b.name; });
+
+    for (auto&& i : info) {
+      if (auto source = gNetworkTableSources[i.entry].get()) {
+        ImGui::Selectable(i.name.c_str());
+        source->EmitDrag();
+      } else {
+        ImGui::Text("%s", i.name.c_str());
+      }
+      ImGui::NextColumn();
+
+      if (auto val = nt::GetEntryValue(i.entry)) {
+        ImGui::PushID(i.name.c_str());
+        switch (val->type()) {
+          case NT_BOOLEAN: {
+            static const char* boolOptions[] = {"false", "true"};
+            int v = val->GetBoolean() ? 1 : 0;
+            if (ImGui::Combo("boolean", &v, boolOptions, 2))
+              nt::SetEntryValue(i.entry, nt::NetworkTableValue::MakeBoolean(v));
+            break;
+          }
+          case NT_DOUBLE: {
+            double v = val->GetDouble();
+            if (ImGui::InputDouble("double", &v, 0, 0, "%.6f",
+                                   ImGuiInputTextFlags_EnterReturnsTrue))
+              nt::SetEntryValue(i.entry, nt::NetworkTableValue::MakeDouble(v));
+            break;
+          }
+          case NT_STRING: {
+            char* v = GetTextBuffer(val->GetString());
+            if (ImGui::InputText("string", v, kTextBufferSize,
+                                 ImGuiInputTextFlags_EnterReturnsTrue))
+              nt::SetEntryValue(i.entry, nt::NetworkTableValue::MakeString(v));
+            break;
+          }
+          case NT_BOOLEAN_ARRAY: {
+            wpi::SmallString<64> buf;
+            BooleanArrayToString(buf, val->GetBooleanArray());
+            char* v = GetTextBuffer(buf);
+            if (ImGui::InputText("boolean[]", v, kTextBufferSize,
+                                 ImGuiInputTextFlags_EnterReturnsTrue)) {
+              if (auto outv = StringToBooleanArray(v))
+                nt::SetEntryValue(i.entry, std::move(outv));
+            }
+            break;
+          }
+          case NT_DOUBLE_ARRAY: {
+            wpi::SmallString<64> buf;
+            DoubleArrayToString(buf, val->GetDoubleArray());
+            char* v = GetTextBuffer(buf);
+            if (ImGui::InputText("double[]", v, kTextBufferSize,
+                                 ImGuiInputTextFlags_EnterReturnsTrue)) {
+              if (auto outv = StringToDoubleArray(v))
+                nt::SetEntryValue(i.entry, std::move(outv));
+            }
+            break;
+          }
+          case NT_STRING_ARRAY: {
+            wpi::SmallString<64> buf;
+            StringArrayToString(buf, val->GetStringArray());
+            char* v = GetTextBuffer(buf);
+            if (ImGui::InputText("string[]", v, kTextBufferSize,
+                                 ImGuiInputTextFlags_EnterReturnsTrue)) {
+              if (auto outv = StringToStringArray(v))
+                nt::SetEntryValue(i.entry, std::move(outv));
+            }
+            break;
+          }
+          case NT_RAW:
+            ImGui::LabelText("raw", "[...]");
+            break;
+          case NT_RPC:
+            ImGui::LabelText("rpc", "[...]");
+            break;
+          default:
+            ImGui::LabelText("other", "?");
+            break;
+        }
+        ImGui::PopID();
+      }
+      ImGui::NextColumn();
+
+      if ((i.flags & NT_PERSISTENT) != 0)
+        ImGui::Text("Persistent");
+      else if (i.flags != 0)
+        ImGui::Text("%02x", i.flags);
+      ImGui::NextColumn();
+
+      ImGui::Text("%llu",
+                  static_cast<unsigned long long>(  // NOLINT(runtime/int)
+                      i.last_change));
+      ImGui::NextColumn();
+      ImGui::Separator();
+    }
+    ImGui::Columns();
+  }
+}
+
+void NetworkTablesGui::Initialize() {
+  gNetworkTablesPoller =
+      nt::CreateEntryListenerPoller(nt::GetDefaultInstance());
+  nt::AddPolledEntryListener(gNetworkTablesPoller, "",
+                             NT_NOTIFY_LOCAL | NT_NOTIFY_NEW |
+                                 NT_NOTIFY_UPDATE | NT_NOTIFY_DELETE |
+                                 NT_NOTIFY_IMMEDIATE);
+  HALSimGui::AddExecute(UpdateNetworkTableSources);
+  HALSimGui::AddWindow("NetworkTables", DisplayNetworkTables);
+  HALSimGui::SetDefaultWindowPos("NetworkTables", 250, 277);
+  HALSimGui::SetDefaultWindowSize("NetworkTables", 750, 185);
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.h b/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.h
new file mode 100644
index 0000000..f4e5d7b
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.h
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+class NetworkTablesGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp b/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
index da29ac3..35ba492 100644
--- a/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,73 +7,134 @@
 
 #include "PDPGui.h"
 
+#include <algorithm>
 #include <cstdio>
+#include <cstring>
 #include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/PDPData.h>
 #include <imgui.h>
-#include <mockdata/PDPData.h>
 
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PDPTemperature, "PDP Temp");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PDPVoltage, "PDP Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED2(PDPCurrent, "PDP Current");
+struct PDPSource {
+  explicit PDPSource(int32_t index) : temp{index}, voltage{index} {
+    const int numChannels = HAL_GetNumPDPChannels();
+    currents.reserve(numChannels);
+    for (int i = 0; i < numChannels; ++i)
+      currents.emplace_back(std::make_unique<PDPCurrentSource>(index, i));
+  }
+  PDPTemperatureSource temp;
+  PDPVoltageSource voltage;
+  std::vector<std::unique_ptr<PDPCurrentSource>> currents;
+};
+}  // namespace
+
+static IniSaver<NameInfo> gChannels{"PDP"};
+static std::vector<std::unique_ptr<PDPSource>> gPDPSources;
+
+static void UpdatePDPSources() {
+  for (int i = 0, iend = gPDPSources.size(); i < iend; ++i) {
+    auto& source = gPDPSources[i];
+    if (HALSIM_GetPDPInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<PDPSource>(i);
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
 static void DisplayPDP() {
   bool hasAny = false;
-  static int numPDP = HAL_GetNumPDPModules();
-  static int numChannels = HAL_GetNumPDPChannels();
-  static auto channelCurrents = std::make_unique<double[]>(numChannels);
-  ImGui::PushItemWidth(ImGui::GetFontSize() * 13);
-  for (int i = 0; i < numPDP; ++i) {
-    if (HALSIM_GetPDPInitialized(i)) {
+  for (int i = 0, iend = gPDPSources.size(); i < iend; ++i) {
+    if (auto source = gPDPSources[i].get()) {
       hasAny = true;
 
-      char name[32];
+      char name[128];
       std::snprintf(name, sizeof(name), "PDP[%d]", i);
       if (ImGui::CollapsingHeader(name, ImGuiTreeNodeFlags_DefaultOpen)) {
         ImGui::PushID(i);
 
         // temperature
-        double temp = HALSIM_GetPDPTemperature(i);
-        if (ImGui::InputDouble("Temp", &temp))
+        double temp = source->temp.GetValue();
+        ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+        if (source->temp.InputDouble("Temp", &temp, 0, 0, "%.3f"))
           HALSIM_SetPDPTemperature(i, temp);
 
         // voltage
-        double volts = HALSIM_GetPDPVoltage(i);
-        if (ImGui::InputDouble("Voltage", &volts))
+        double volts = source->voltage.GetValue();
+        ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+        if (source->voltage.InputDouble("Voltage", &volts, 0, 0, "%.3f"))
           HALSIM_SetPDPVoltage(i, volts);
 
         // channel currents; show as two columns laid out like PDP
-        HALSIM_GetPDPAllCurrents(i, channelCurrents.get());
+        const int numChannels = source->currents.size();
         ImGui::Text("Channel Current (A)");
         ImGui::Columns(2, "channels", false);
+        float maxWidth = ImGui::GetFontSize() * 13;
         for (int left = 0, right = numChannels - 1; left < right;
              ++left, --right) {
           double val;
 
-          std::snprintf(name, sizeof(name), "[%d]", left);
-          val = channelCurrents[left];
-          if (ImGui::InputDouble(name, &val))
+          ImGui::PushID(left);
+          auto& leftInfo = gChannels[i * numChannels + left];
+          leftInfo.GetLabel(name, sizeof(name), "", left);
+          val = source->currents[left]->GetValue();
+          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+          if (source->currents[left]->InputDouble(name, &val, 0, 0, "%.3f"))
             HALSIM_SetPDPCurrent(i, left, val);
+          float leftWidth = ImGui::GetItemRectSize().x;
+          if (leftInfo.PopupEditName(left)) {
+            source->currents[left]->SetName(leftInfo.GetName());
+          }
+          ImGui::PopID();
           ImGui::NextColumn();
 
-          std::snprintf(name, sizeof(name), "[%d]", right);
-          val = channelCurrents[right];
-          if (ImGui::InputDouble(name, &val))
+          ImGui::PushID(right);
+          auto& rightInfo = gChannels[i * numChannels + right];
+          rightInfo.GetLabel(name, sizeof(name), "", right);
+          val = source->currents[right]->GetValue();
+          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+          if (source->currents[right]->InputDouble(name, &val, 0, 0, "%.3f"))
             HALSIM_SetPDPCurrent(i, right, val);
+          float rightWidth = ImGui::GetItemRectSize().x;
+          if (rightInfo.PopupEditName(right)) {
+            source->currents[right]->SetName(rightInfo.GetName());
+          }
+          ImGui::PopID();
           ImGui::NextColumn();
+
+          float width =
+              (std::max)(leftWidth, rightWidth) * 2 + ImGui::GetFontSize() * 4;
+          if (width > maxWidth) maxWidth = width;
         }
         ImGui::Columns(1);
+        ImGui::Dummy(ImVec2(maxWidth, 0));
         ImGui::PopID();
       }
     }
   }
-  ImGui::PopItemWidth();
   if (!hasAny) ImGui::Text("No PDPs");
 }
 
 void PDPGui::Initialize() {
-  HALSimGui::AddWindow("PDP", DisplayPDP, ImGuiWindowFlags_AlwaysAutoResize);
+  gChannels.Initialize();
+  gPDPSources.resize(HAL_GetNumPDPModules());
+  HALSimGui::AddExecute(UpdatePDPSources);
+  HALSimGui::AddWindow("PDP", DisplayPDP);
   // hide it by default
   HALSimGui::SetWindowVisibility("PDP", HALSimGui::kHide);
   HALSimGui::SetDefaultWindowPos("PDP", 245, 155);

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp b/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
index 430502d..59d7a93 100644
--- a/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,16 +10,45 @@
 #include <cstdio>
 #include <cstring>
 #include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/AddressableLEDData.h>
+#include <hal/simulation/PWMData.h>
 #include <imgui.h>
-#include <mockdata/AddressableLEDData.h>
-#include <mockdata/PWMData.h>
 
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PWMSpeed, "PWM");
+}  // namespace
+
+static IniSaver<NameInfo> gPWM{"PWM"};
+static std::vector<std::unique_ptr<PWMSpeedSource>> gPWMSources;
+
+static void UpdatePWMSources() {
+  static const int numPWM = HAL_GetNumPWMChannels();
+  if (static_cast<size_t>(numPWM) != gPWMSources.size())
+    gPWMSources.resize(numPWM);
+
+  for (int i = 0; i < numPWM; ++i) {
+    auto& source = gPWMSources[i];
+    if (HALSIM_GetPWMInitialized(i)) {
+      if (!source) {
+        source = std::make_unique<PWMSpeedSource>(i);
+        source->SetName(gPWM[i].GetName());
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
 static void DisplayPWMs() {
   bool hasOutputs = false;
   static const int numPWM = HAL_GetNumPWMChannels();
@@ -35,18 +64,11 @@
     }
   }
 
-  // struct History {
-  //  History() { std::memset(data, 0, 90 * sizeof(float)); }
-  //  History(const History&) = delete;
-  //  History& operator=(const History&) = delete;
-  //  float data[90];
-  //  int display_offset = 0;
-  //  int save_offset = 0;
-  //};
-  // static std::vector<std::unique_ptr<History>> history;
   bool first = true;
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
   for (int i = 0; i < numPWM; ++i) {
-    if (HALSIM_GetPWMInitialized(i)) {
+    if (auto source = gPWMSources[i].get()) {
+      ImGui::PushID(i);
       hasOutputs = true;
 
       if (!first)
@@ -54,30 +76,28 @@
       else
         first = false;
 
-      char name[32];
-      std::snprintf(name, sizeof(name), "PWM[%d]", i);
+      auto& info = gPWM[i];
+      char label[128];
+      info.GetLabel(label, sizeof(label), "PWM", i);
       if (ledMap[i] > 0) {
-        ImGui::Text("%s: LED[%d]", name, ledMap[i] - 1);
+        ImGui::LabelText(label, "LED[%d]", ledMap[i] - 1);
       } else {
         float val = HALSimGui::AreOutputsDisabled() ? 0 : HALSIM_GetPWMSpeed(i);
-        ImGui::Value(name, val, "%0.3f");
+        source->LabelText(label, "%0.3f", val);
       }
-
-      // lazily build history storage
-      // if (static_cast<unsigned int>(i) > history.size())
-      //  history.resize(i + 1);
-      // if (!history[i]) history[i] = std::make_unique<History>();
-
-      // save history
-
-      // ImGui::PlotLines(labels[i].c_str(), values.data(), values.size(),
-      // );
+      if (info.PopupEditName(i)) {
+        source->SetName(info.GetName());
+      }
+      ImGui::PopID();
     }
   }
+  ImGui::PopItemWidth();
   if (!hasOutputs) ImGui::Text("No PWM outputs");
 }
 
 void PWMGui::Initialize() {
+  gPWM.Initialize();
+  HALSimGui::AddExecute(UpdatePWMSources);
   HALSimGui::AddWindow("PWM Outputs", DisplayPWMs,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("PWM Outputs", 910, 20);

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/PlotGui.cpp b/simulation/halsim_gui/src/main/native/cpp/PlotGui.cpp
new file mode 100644
index 0000000..ecaec63
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/PlotGui.cpp
@@ -0,0 +1,894 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "PlotGui.h"
+
+#include <algorithm>
+#include <atomic>
+#include <cstdio>
+#include <cstring>
+#include <memory>
+#include <vector>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <hal/simulation/MockHooks.h>
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <implot.h>
+#include <wpi/Signal.h>
+#include <wpi/SmallVector.h>
+#include <wpi/timestamp.h>
+#include <wpi/raw_ostream.h>
+
+#include "GuiDataSource.h"
+#include "HALSimGui.h"
+#include "IniSaverInfo.h"
+#include "IniSaverVector.h"
+
+using namespace halsimgui;
+
+namespace {
+struct PlotSeriesRef {
+  size_t plotIndex;
+  size_t seriesIndex;
+};
+
+class PlotSeries : public NameInfo, public OpenInfo {
+ public:
+  explicit PlotSeries(wpi::StringRef id);
+  explicit PlotSeries(GuiDataSource* source, int yAxis = 0);
+
+  const std::string& GetId() const { return m_id; }
+
+  void CheckSource();
+
+  void SetSource(GuiDataSource* source);
+  GuiDataSource* GetSource() const { return m_source; }
+
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
+  void WriteIni(ImGuiTextBuffer* out);
+
+  bool EmitPlot(double now, size_t i, size_t plotIndex);
+  bool EmitSettings(size_t i, size_t plotIndex);
+  bool EmitSettingsDetail(size_t i);
+  void EmitDragDropPayload(size_t i, size_t plotIndex);
+
+  void GetLabel(char* buf, size_t size) const;
+
+  int GetYAxis() const { return m_yAxis; }
+  void SetYAxis(int yAxis) { m_yAxis = yAxis; }
+
+ private:
+  bool IsDigital() const {
+    return m_digital == kDigital ||
+           (m_digital == kAuto && m_source && m_source->IsDigital());
+  }
+  void AppendValue(double value);
+
+  // source linkage
+  GuiDataSource* m_source = nullptr;
+  wpi::sig::ScopedConnection m_sourceCreatedConn;
+  wpi::sig::ScopedConnection m_newValueConn;
+  std::string m_id;
+
+  // user settings
+  int m_yAxis = 0;
+  ImVec4 m_color = IMPLOT_AUTO_COL;
+  int m_marker = 0;
+
+  enum Digital { kAuto, kDigital, kAnalog };
+  int m_digital = 0;
+  int m_digitalBitHeight = 8;
+  int m_digitalBitGap = 4;
+
+  // value storage
+  static constexpr int kMaxSize = 2000;
+  static constexpr double kTimeGap = 0.05;
+  std::atomic<int> m_size = 0;
+  std::atomic<int> m_offset = 0;
+  ImPlotPoint m_data[kMaxSize];
+};
+
+class Plot : public NameInfo, public OpenInfo {
+ public:
+  Plot();
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
+  void WriteIni(ImGuiTextBuffer* out);
+
+  void GetLabel(char* buf, size_t size, int index) const;
+  void GetName(char* buf, size_t size, int index) const;
+
+  void DragDropTarget(size_t i, bool inPlot);
+  void EmitPlot(double now, size_t i);
+  void EmitSettings(size_t i);
+
+  std::vector<std::unique_ptr<PlotSeries>> m_series;
+
+ private:
+  void EmitSettingsLimits(int axis);
+
+  bool m_visible = true;
+  unsigned int m_plotFlags = ImPlotFlags_Default;
+  bool m_lockPrevX = false;
+  bool m_paused = false;
+  float m_viewTime = 10;
+  int m_height = 300;
+  struct PlotRange {
+    double min = 0;
+    double max = 1;
+    bool lockMin = false;
+    bool lockMax = false;
+    bool apply = false;
+  };
+  PlotRange m_axisRange[3];
+  ImPlotRange m_xaxisRange;  // read from plot, used for lockPrevX
+};
+}  // namespace
+
+static IniSaverVector<Plot> gPlots{"Plot"};
+
+PlotSeries::PlotSeries(wpi::StringRef id) : m_id(id) {
+  if (GuiDataSource* source = GuiDataSource::Find(id)) {
+    SetSource(source);
+    return;
+  }
+  CheckSource();
+}
+
+PlotSeries::PlotSeries(GuiDataSource* source, int yAxis) : m_yAxis(yAxis) {
+  SetSource(source);
+}
+
+void PlotSeries::CheckSource() {
+  if (!m_newValueConn.connected() && !m_sourceCreatedConn.connected()) {
+    m_source = nullptr;
+    m_sourceCreatedConn = GuiDataSource::sourceCreated.connect_connection(
+        [this](const char* id, GuiDataSource* source) {
+          if (m_id == id) {
+            SetSource(source);
+            m_sourceCreatedConn.disconnect();
+          }
+        });
+  }
+}
+
+void PlotSeries::SetSource(GuiDataSource* source) {
+  m_source = source;
+  m_id = source->GetId();
+
+  // add initial value
+  m_data[m_size++] = ImPlotPoint{wpi::Now() * 1.0e-6, source->GetValue()};
+
+  m_newValueConn = source->valueChanged.connect_connection(
+      [this](double value) { AppendValue(value); });
+}
+
+void PlotSeries::AppendValue(double value) {
+  double time = wpi::Now() * 1.0e-6;
+  if (IsDigital()) {
+    if (m_size < kMaxSize) {
+      m_data[m_size] = ImPlotPoint{time, value};
+      ++m_size;
+    } else {
+      m_data[m_offset] = ImPlotPoint{time, value};
+      m_offset = (m_offset + 1) % kMaxSize;
+    }
+  } else {
+    // as an analog graph draws linear lines in between each value,
+    // insert duplicate value if "long" time between updates so it
+    // looks appropriately flat
+    if (m_size < kMaxSize) {
+      if (m_size > 0) {
+        if ((time - m_data[m_size - 1].x) > kTimeGap) {
+          m_data[m_size] = ImPlotPoint{time, m_data[m_size - 1].y};
+          ++m_size;
+        }
+      }
+      m_data[m_size] = ImPlotPoint{time, value};
+      ++m_size;
+    } else {
+      if (m_offset == 0) {
+        if ((time - m_data[kMaxSize - 1].x) > kTimeGap) {
+          m_data[m_offset] = ImPlotPoint{time, m_data[kMaxSize - 1].y};
+          ++m_offset;
+        }
+      } else {
+        if ((time - m_data[m_offset - 1].x) > kTimeGap) {
+          m_data[m_offset] = ImPlotPoint{time, m_data[m_offset - 1].y};
+          m_offset = (m_offset + 1) % kMaxSize;
+        }
+      }
+      m_data[m_offset] = ImPlotPoint{time, value};
+      m_offset = (m_offset + 1) % kMaxSize;
+    }
+  }
+}
+
+bool PlotSeries::ReadIni(wpi::StringRef name, wpi::StringRef value) {
+  if (NameInfo::ReadIni(name, value)) return true;
+  if (OpenInfo::ReadIni(name, value)) return true;
+  if (name == "yAxis") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_yAxis = num;
+    return true;
+  } else if (name == "color") {
+    unsigned int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_color = ImColor(num);
+    return true;
+  } else if (name == "marker") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_marker = num;
+    return true;
+  } else if (name == "digital") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_digital = num;
+    return true;
+  } else if (name == "digitalBitHeight") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_digitalBitHeight = num;
+    return true;
+  } else if (name == "digitalBitGap") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_digitalBitGap = num;
+    return true;
+  }
+  return false;
+}
+
+void PlotSeries::WriteIni(ImGuiTextBuffer* out) {
+  NameInfo::WriteIni(out);
+  OpenInfo::WriteIni(out);
+  out->appendf(
+      "yAxis=%d\ncolor=%u\nmarker=%d\ndigital=%d\n"
+      "digitalBitHeight=%d\ndigitalBitGap=%d\n",
+      m_yAxis, static_cast<ImU32>(ImColor(m_color)), m_marker, m_digital,
+      m_digitalBitHeight, m_digitalBitGap);
+}
+
+void PlotSeries::GetLabel(char* buf, size_t size) const {
+  const char* name = GetName();
+  if (name[0] == '\0' && m_newValueConn.connected()) name = m_source->GetName();
+  if (name[0] == '\0') name = m_id.c_str();
+  std::snprintf(buf, size, "%s###%s", name, m_id.c_str());
+}
+
+bool PlotSeries::EmitPlot(double now, size_t i, size_t plotIndex) {
+  CheckSource();
+
+  char label[128];
+  GetLabel(label, sizeof(label));
+
+  int size = m_size;
+  int offset = m_offset;
+
+  // need to have last value at current time, so need to create fake last value
+  // we handle the offset logic ourselves to avoid wrap issues with size + 1
+  struct GetterData {
+    double now;
+    ImPlotPoint* data;
+    int size;
+    int offset;
+  };
+  GetterData getterData = {now, m_data, size, offset};
+  auto getter = [](void* data, int idx) {
+    auto d = static_cast<GetterData*>(data);
+    if (idx == d->size)
+      return ImPlotPoint{
+          d->now, d->data[d->offset == 0 ? d->size - 1 : d->offset - 1].y};
+    if (d->offset + idx < d->size)
+      return d->data[d->offset + idx];
+    else
+      return d->data[d->offset + idx - d->size];
+  };
+
+  if (m_color.w == IMPLOT_AUTO_COL.w) m_color = ImPlot::GetColormapColor(i);
+  ImPlot::PushStyleColor(ImPlotCol_Line, m_color);
+  if (IsDigital()) {
+    ImPlot::PushStyleVar(ImPlotStyleVar_DigitalBitHeight, m_digitalBitHeight);
+    ImPlot::PushStyleVar(ImPlotStyleVar_DigitalBitGap, m_digitalBitGap);
+    ImPlot::PlotDigital(label, getter, &getterData, size + 1);
+    ImPlot::PopStyleVar();
+    ImPlot::PopStyleVar();
+  } else {
+    ImPlot::SetPlotYAxis(m_yAxis);
+    ImPlot::SetNextMarkerStyle(m_marker - 1);
+    ImPlot::PlotLine(label, getter, &getterData, size + 1);
+  }
+  ImPlot::PopStyleColor();
+
+  // DND source for PlotSeries
+  if (ImPlot::BeginLegendDragDropSource(label)) {
+    EmitDragDropPayload(i, plotIndex);
+    ImPlot::EndLegendDragDropSource();
+  }
+
+  // Plot-specific variant of IniSaverInfo::PopupEditName() that also
+  // allows editing of other settings
+  bool rv = false;
+  if (ImPlot::BeginLegendPopup(label)) {
+    if (ImGui::Button("Close")) ImGui::CloseCurrentPopup();
+    ImGui::Text("Edit name:");
+    if (InputTextName("##edit", ImGuiInputTextFlags_EnterReturnsTrue)) {
+      ImGui::CloseCurrentPopup();
+    }
+    rv = EmitSettingsDetail(i);
+    ImPlot::EndLegendPopup();
+  }
+
+  return rv;
+}
+
+void PlotSeries::EmitDragDropPayload(size_t i, size_t plotIndex) {
+  PlotSeriesRef ref = {plotIndex, i};
+  ImGui::SetDragDropPayload("PlotSeries", &ref, sizeof(ref));
+  const char* name = GetName();
+  if (name[0] == '\0' && m_newValueConn.connected()) name = m_source->GetName();
+  if (name[0] == '\0') name = m_id.c_str();
+  ImGui::TextUnformatted(name);
+}
+
+static void MovePlotSeries(size_t fromPlotIndex, size_t fromSeriesIndex,
+                           size_t toPlotIndex, size_t toSeriesIndex,
+                           int yAxis = -1) {
+  if (fromPlotIndex == toPlotIndex) {
+    // need to handle this specially as the index of the old location changes
+    if (fromSeriesIndex != toSeriesIndex) {
+      auto& plotSeries = gPlots[fromPlotIndex].m_series;
+      auto val = std::move(plotSeries[fromSeriesIndex]);
+      // only set Y-axis if actually set
+      if (yAxis != -1) val->SetYAxis(yAxis);
+      plotSeries.insert(plotSeries.begin() + toSeriesIndex, std::move(val));
+      plotSeries.erase(plotSeries.begin() + fromSeriesIndex +
+                       (fromSeriesIndex > toSeriesIndex ? 1 : 0));
+    }
+  } else {
+    auto& fromPlot = gPlots[fromPlotIndex];
+    auto& toPlot = gPlots[toPlotIndex];
+    // always set Y-axis if moving plots
+    fromPlot.m_series[fromSeriesIndex]->SetYAxis(yAxis == -1 ? 0 : yAxis);
+    toPlot.m_series.insert(toPlot.m_series.begin() + toSeriesIndex,
+                           std::move(fromPlot.m_series[fromSeriesIndex]));
+    fromPlot.m_series.erase(fromPlot.m_series.begin() + fromSeriesIndex);
+  }
+}
+
+bool PlotSeries::EmitSettings(size_t i, size_t plotIndex) {
+  char label[128];
+  GetLabel(label, sizeof(label));
+
+  bool open = ImGui::CollapsingHeader(
+      label, IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
+
+  // DND source for PlotSeries
+  if (ImGui::BeginDragDropSource()) {
+    EmitDragDropPayload(i, plotIndex);
+    ImGui::EndDragDropSource();
+  }
+
+  // If another PlotSeries is dropped, move it before this series
+  if (ImGui::BeginDragDropTarget()) {
+    if (const ImGuiPayload* payload =
+            ImGui::AcceptDragDropPayload("PlotSeries")) {
+      auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
+      MovePlotSeries(ref->plotIndex, ref->seriesIndex, plotIndex, i);
+    }
+  }
+
+  SetOpen(open);
+  PopupEditName(i);
+  if (!open) return false;
+
+  return EmitSettingsDetail(i);
+}
+
+bool PlotSeries::EmitSettingsDetail(size_t i) {
+  if (ImGui::Button("Delete")) {
+    return true;
+  }
+
+  // Line color
+  {
+    ImGui::ColorEdit3("Color", &m_color.x, ImGuiColorEditFlags_NoInputs);
+    ImGui::SameLine();
+    if (ImGui::Button("Default")) m_color = ImPlot::GetColormapColor(i);
+  }
+
+  // Digital
+  {
+    static const char* const options[] = {"Auto", "Digital", "Analog"};
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+    ImGui::Combo("Digital", &m_digital, options,
+                 sizeof(options) / sizeof(options[0]));
+  }
+
+  if (IsDigital()) {
+    // Bit Height
+    {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      ImGui::InputInt("Bit Height", &m_digitalBitHeight);
+    }
+
+    // Bit Gap
+    {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      ImGui::InputInt("Bit Gap", &m_digitalBitGap);
+    }
+  } else {
+    // Y-axis
+    {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      static const char* const options[] = {"1", "2", "3"};
+      ImGui::Combo("Y-Axis", &m_yAxis, options, 3);
+    }
+
+    // Marker
+    {
+      static const char* const options[] = {
+          "None", "Circle", "Square", "Diamond", "Up",      "Down",
+          "Left", "Right",  "Cross",  "Plus",    "Asterisk"};
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+      ImGui::Combo("Marker", &m_marker, options,
+                   sizeof(options) / sizeof(options[0]));
+    }
+  }
+
+  return false;
+}
+
+Plot::Plot() {
+  for (int i = 0; i < 3; ++i) {
+    m_axisRange[i] = PlotRange{};
+  }
+}
+
+bool Plot::ReadIni(wpi::StringRef name, wpi::StringRef value) {
+  if (NameInfo::ReadIni(name, value)) return true;
+  if (OpenInfo::ReadIni(name, value)) return true;
+  if (name == "visible") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_visible = num != 0;
+    return true;
+  } else if (name == "lockPrevX") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_lockPrevX = num != 0;
+    return true;
+  } else if (name == "legend") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    if (num == 0)
+      m_plotFlags &= ~ImPlotFlags_Legend;
+    else
+      m_plotFlags |= ImPlotFlags_Legend;
+    return true;
+  } else if (name == "yaxis2") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    if (num == 0)
+      m_plotFlags &= ~ImPlotFlags_YAxis2;
+    else
+      m_plotFlags |= ImPlotFlags_YAxis2;
+    return true;
+  } else if (name == "yaxis3") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    if (num == 0)
+      m_plotFlags &= ~ImPlotFlags_YAxis3;
+    else
+      m_plotFlags |= ImPlotFlags_YAxis3;
+    return true;
+  } else if (name == "viewTime") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_viewTime = num / 1000.0;
+    return true;
+  } else if (name == "height") {
+    int num;
+    if (value.getAsInteger(10, num)) return true;
+    m_height = num;
+    return true;
+  } else if (name.startswith("y")) {
+    auto [yAxisStr, yName] = name.split('_');
+    int yAxis;
+    if (yAxisStr.substr(1).getAsInteger(10, yAxis)) return false;
+    if (yAxis < 0 || yAxis > 3) return false;
+    if (yName == "min") {
+      int num;
+      if (value.getAsInteger(10, num)) return true;
+      m_axisRange[yAxis].min = num / 1000.0;
+      return true;
+    } else if (yName == "max") {
+      int num;
+      if (value.getAsInteger(10, num)) return true;
+      m_axisRange[yAxis].max = num / 1000.0;
+      return true;
+    } else if (yName == "lockMin") {
+      int num;
+      if (value.getAsInteger(10, num)) return true;
+      m_axisRange[yAxis].lockMin = num != 0;
+      return true;
+    } else if (yName == "lockMax") {
+      int num;
+      if (value.getAsInteger(10, num)) return true;
+      m_axisRange[yAxis].lockMax = num != 0;
+      return true;
+    }
+  }
+  return false;
+}
+
+void Plot::WriteIni(ImGuiTextBuffer* out) {
+  NameInfo::WriteIni(out);
+  OpenInfo::WriteIni(out);
+  out->appendf(
+      "visible=%d\nlockPrevX=%d\nlegend=%d\nyaxis2=%d\nyaxis3=%d\n"
+      "viewTime=%d\nheight=%d\n",
+      m_visible ? 1 : 0, m_lockPrevX ? 1 : 0,
+      (m_plotFlags & ImPlotFlags_Legend) ? 1 : 0,
+      (m_plotFlags & ImPlotFlags_YAxis2) ? 1 : 0,
+      (m_plotFlags & ImPlotFlags_YAxis3) ? 1 : 0,
+      static_cast<int>(m_viewTime * 1000), m_height);
+  for (int i = 0; i < 3; ++i) {
+    out->appendf("y%d_min=%d\ny%d_max=%d\ny%d_lockMin=%d\ny%d_lockMax=%d\n", i,
+                 static_cast<int>(m_axisRange[i].min * 1000), i,
+                 static_cast<int>(m_axisRange[i].max * 1000), i,
+                 m_axisRange[i].lockMin ? 1 : 0, i,
+                 m_axisRange[i].lockMax ? 1 : 0);
+  }
+}
+
+void Plot::GetLabel(char* buf, size_t size, int index) const {
+  const char* name = NameInfo::GetName();
+  if (name[0] != '\0') {
+    std::snprintf(buf, size, "%s##Plot%d", name, index);
+  } else {
+    std::snprintf(buf, size, "Plot %d##Plot%d", index, index);
+  }
+}
+
+void Plot::GetName(char* buf, size_t size, int index) const {
+  const char* name = NameInfo::GetName();
+  if (name[0] != '\0') {
+    std::snprintf(buf, size, "%s", name);
+  } else {
+    std::snprintf(buf, size, "Plot %d", index);
+  }
+}
+
+void Plot::DragDropTarget(size_t i, bool inPlot) {
+  if (!ImGui::BeginDragDropTarget()) return;
+  // handle dragging onto a specific Y axis
+  int yAxis = -1;
+  if (inPlot) {
+    for (int y = 0; y < 3; ++y) {
+      if (ImPlot::IsPlotYAxisHovered(y)) {
+        yAxis = y;
+        break;
+      }
+    }
+  }
+  if (const ImGuiPayload* payload =
+          ImGui::AcceptDragDropPayload("DataSource")) {
+    auto source = *static_cast<GuiDataSource**>(payload->Data);
+    // don't add duplicates unless it's onto a different Y axis
+    auto it =
+        std::find_if(m_series.begin(), m_series.end(), [=](const auto& elem) {
+          return elem->GetId() == source->GetId() &&
+                 (yAxis == -1 || elem->GetYAxis() == yAxis);
+        });
+    if (it == m_series.end()) {
+      m_series.emplace_back(
+          std::make_unique<PlotSeries>(source, yAxis == -1 ? 0 : yAxis));
+    }
+  } else if (const ImGuiPayload* payload =
+                 ImGui::AcceptDragDropPayload("PlotSeries")) {
+    auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
+    MovePlotSeries(ref->plotIndex, ref->seriesIndex, i, m_series.size(), yAxis);
+  } else if (const ImGuiPayload* payload =
+                 ImGui::AcceptDragDropPayload("Plot")) {
+    auto fromPlotIndex = *static_cast<const size_t*>(payload->Data);
+    if (i != fromPlotIndex) {
+      auto val = std::move(gPlots[fromPlotIndex]);
+      gPlots.insert(gPlots.begin() + i, std::move(val));
+      gPlots.erase(gPlots.begin() + fromPlotIndex +
+                   (fromPlotIndex > i ? 1 : 0));
+    }
+  }
+}
+
+void Plot::EmitPlot(double now, size_t i) {
+  if (!m_visible) return;
+
+  bool lockX = (i != 0 && m_lockPrevX);
+
+  if (!lockX && ImGui::Button(m_paused ? "Resume" : "Pause"))
+    m_paused = !m_paused;
+
+  char label[128];
+  GetLabel(label, sizeof(label), i);
+
+  if (lockX) {
+    ImPlot::SetNextPlotLimitsX(gPlots[i - 1].m_xaxisRange.Min,
+                               gPlots[i - 1].m_xaxisRange.Max,
+                               ImGuiCond_Always);
+  } else {
+    // also force-pause plots if overall timing is paused
+    ImPlot::SetNextPlotLimitsX(now - m_viewTime, now,
+                               (m_paused || HALSIM_IsTimingPaused())
+                                   ? ImGuiCond_Once
+                                   : ImGuiCond_Always);
+  }
+
+  ImPlotAxisFlags yFlags[3] = {ImPlotAxisFlags_Default,
+                               ImPlotAxisFlags_Auxiliary,
+                               ImPlotAxisFlags_Auxiliary};
+  for (int i = 0; i < 3; ++i) {
+    ImPlot::SetNextPlotLimitsY(
+        m_axisRange[i].min, m_axisRange[i].max,
+        m_axisRange[i].apply ? ImGuiCond_Always : ImGuiCond_Once, i);
+    m_axisRange[i].apply = false;
+    if (m_axisRange[i].lockMin) yFlags[i] |= ImPlotAxisFlags_LockMin;
+    if (m_axisRange[i].lockMax) yFlags[i] |= ImPlotAxisFlags_LockMax;
+  }
+
+  if (ImPlot::BeginPlot(label, nullptr, nullptr, ImVec2(-1, m_height),
+                        m_plotFlags, ImPlotAxisFlags_Default, yFlags[0],
+                        yFlags[1], yFlags[2])) {
+    for (size_t j = 0; j < m_series.size(); ++j) {
+      if (m_series[j]->EmitPlot(now, j, i)) {
+        m_series.erase(m_series.begin() + j);
+      }
+    }
+    DragDropTarget(i, true);
+    m_xaxisRange = ImPlot::GetPlotLimits().X;
+    ImPlot::EndPlot();
+  }
+}
+
+void Plot::EmitSettingsLimits(int axis) {
+  ImGui::Indent();
+  ImGui::PushID(axis);
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 3.5);
+  ImGui::InputDouble("Min", &m_axisRange[axis].min, 0, 0, "%.3f");
+  ImGui::SameLine();
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 3.5);
+  ImGui::InputDouble("Max", &m_axisRange[axis].max, 0, 0, "%.3f");
+  ImGui::SameLine();
+  if (ImGui::Button("Apply")) m_axisRange[axis].apply = true;
+
+  ImGui::TextUnformatted("Lock Axis");
+  ImGui::SameLine();
+  ImGui::Checkbox("Min##minlock", &m_axisRange[axis].lockMin);
+  ImGui::SameLine();
+  ImGui::Checkbox("Max##maxlock", &m_axisRange[axis].lockMax);
+
+  ImGui::PopID();
+  ImGui::Unindent();
+}
+
+// Delete button (X in circle), based on ImGui::CloseButton()
+static bool DeleteButton(ImGuiID id, const ImVec2& pos) {
+  ImGuiContext& g = *GImGui;
+  ImGuiWindow* window = g.CurrentWindow;
+
+  // We intentionally allow interaction when clipped so that a mechanical
+  // Alt,Right,Validate sequence close a window. (this isn't the regular
+  // behavior of buttons, but it doesn't affect the user much because navigation
+  // tends to keep items visible).
+  const ImRect bb(
+      pos, pos + ImVec2(g.FontSize, g.FontSize) + g.Style.FramePadding * 2.0f);
+  bool is_clipped = !ImGui::ItemAdd(bb, id);
+
+  bool hovered, held;
+  bool pressed = ImGui::ButtonBehavior(bb, id, &hovered, &held);
+  if (is_clipped) return pressed;
+
+  // Render
+  ImU32 col =
+      ImGui::GetColorU32(held ? ImGuiCol_ButtonActive : ImGuiCol_ButtonHovered);
+  ImVec2 center = bb.GetCenter();
+  if (hovered)
+    window->DrawList->AddCircleFilled(
+        center, ImMax(2.0f, g.FontSize * 0.5f + 1.0f), col, 12);
+
+  ImU32 cross_col = ImGui::GetColorU32(ImGuiCol_Text);
+  window->DrawList->AddCircle(center, ImMax(2.0f, g.FontSize * 0.5f + 1.0f),
+                              cross_col, 12);
+  float cross_extent = g.FontSize * 0.5f * 0.5f - 1.0f;
+  center -= ImVec2(0.5f, 0.5f);
+  window->DrawList->AddLine(center + ImVec2(+cross_extent, +cross_extent),
+                            center + ImVec2(-cross_extent, -cross_extent),
+                            cross_col, 1.0f);
+  window->DrawList->AddLine(center + ImVec2(+cross_extent, -cross_extent),
+                            center + ImVec2(-cross_extent, +cross_extent),
+                            cross_col, 1.0f);
+
+  return pressed;
+}
+
+void Plot::EmitSettings(size_t i) {
+  char label[128];
+  GetLabel(label, sizeof(label), i);
+
+  bool open = ImGui::CollapsingHeader(
+      label, ImGuiTreeNodeFlags_AllowItemOverlap |
+                 ImGuiTreeNodeFlags_ClipLabelForTrailingButton |
+                 (IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0));
+
+  {
+    // Create a small overlapping delete button
+    ImGuiWindow* window = ImGui::GetCurrentWindow();
+    ImGuiContext& g = *GImGui;
+    ImGuiItemHoveredDataBackup last_item_backup;
+    ImGuiID id = window->GetID(label);
+    float button_size = g.FontSize;
+    float button_x = ImMax(window->DC.LastItemRect.Min.x,
+                           window->DC.LastItemRect.Max.x -
+                               g.Style.FramePadding.x * 2.0f - button_size);
+    float button_y = window->DC.LastItemRect.Min.y;
+    if (DeleteButton(window->GetID(reinterpret_cast<void*>(
+                         static_cast<intptr_t>(id) + 1)),
+                     ImVec2(button_x, button_y))) {
+      gPlots.erase(gPlots.begin() + i);
+      return;
+    }
+    last_item_backup.Restore();
+  }
+
+  // DND source for Plot
+  if (ImGui::BeginDragDropSource()) {
+    ImGui::SetDragDropPayload("Plot", &i, sizeof(i));
+    char name[64];
+    GetName(name, sizeof(name), i);
+    ImGui::TextUnformatted(name);
+    ImGui::EndDragDropSource();
+  }
+  DragDropTarget(i, false);
+  SetOpen(open);
+  PopupEditName(i);
+  if (!open) return;
+  ImGui::PushID(i);
+#if 0
+  if (ImGui::Button("Move Up") && i > 0) {
+    std::swap(gPlots[i - 1], gPlots[i]);
+    ImGui::PopID();
+    return;
+  }
+  ImGui::SameLine();
+  if (ImGui::Button("Move Down") && i < (gPlots.size() - 1)) {
+    std::swap(gPlots[i], gPlots[i + 1]);
+    ImGui::PopID();
+    return;
+  }
+  ImGui::SameLine();
+  if (ImGui::Button("Delete")) {
+    gPlots.erase(gPlots.begin() + i);
+    ImGui::PopID();
+    return;
+  }
+#endif
+  ImGui::Checkbox("Visible", &m_visible);
+  ImGui::CheckboxFlags("Show Legend", &m_plotFlags, ImPlotFlags_Legend);
+  if (i != 0) ImGui::Checkbox("Lock X-axis to previous plot", &m_lockPrevX);
+  ImGui::TextUnformatted("Primary Y-Axis");
+  EmitSettingsLimits(0);
+  ImGui::CheckboxFlags("2nd Y-Axis", &m_plotFlags, ImPlotFlags_YAxis2);
+  if ((m_plotFlags & ImPlotFlags_YAxis2) != 0) EmitSettingsLimits(1);
+  ImGui::CheckboxFlags("3rd Y-Axis", &m_plotFlags, ImPlotFlags_YAxis3);
+  if ((m_plotFlags & ImPlotFlags_YAxis3) != 0) EmitSettingsLimits(2);
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+  ImGui::InputFloat("View Time (s)", &m_viewTime, 0.1f, 1.0f, "%.1f");
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+  if (ImGui::InputInt("Height", &m_height, 10)) {
+    if (m_height < 0) m_height = 0;
+  }
+
+  ImGui::Indent();
+  for (size_t j = 0; j < m_series.size(); ++j) {
+    ImGui::PushID(j);
+    if (m_series[j]->EmitSettings(j, i)) {
+      m_series.erase(m_series.begin() + j);
+    }
+    ImGui::PopID();
+  }
+  ImGui::Unindent();
+
+  ImGui::PopID();
+}
+
+static void DisplayPlot() {
+  if (gPlots.empty()) {
+    ImGui::Text("No Plots");
+    return;
+  }
+  double now = wpi::Now() * 1.0e-6;
+  for (size_t i = 0; i < gPlots.size(); ++i) {
+    ImGui::PushID(i);
+    gPlots[i].EmitPlot(now, i);
+    ImGui::PopID();
+  }
+  ImGui::Text("(Right double click for more settings)");
+}
+
+static void DisplayPlotSettings() {
+  if (ImGui::Button("Add new plot")) {
+    gPlots.emplace_back();
+  }
+  for (size_t i = 0; i < gPlots.size(); ++i) {
+    gPlots[i].EmitSettings(i);
+  }
+}
+
+static void* PlotSeries_ReadOpen(ImGuiContext* ctx,
+                                 ImGuiSettingsHandler* handler,
+                                 const char* name) {
+  wpi::StringRef plotIndexStr, id;
+  std::tie(plotIndexStr, id) = wpi::StringRef{name}.split(',');
+  unsigned int plotIndex;
+  if (plotIndexStr.getAsInteger(10, plotIndex)) return nullptr;
+  if (plotIndex >= gPlots.size()) gPlots.resize(plotIndex + 1);
+  auto& plot = gPlots[plotIndex];
+  auto it = std::find_if(
+      plot.m_series.begin(), plot.m_series.end(),
+      [&](const auto& elem) { return elem && elem->GetId() == id; });
+  if (it != plot.m_series.end()) return it->get();
+  return plot.m_series.emplace_back(std::make_unique<PlotSeries>(id)).get();
+}
+
+static void PlotSeries_ReadLine(ImGuiContext* ctx,
+                                ImGuiSettingsHandler* handler, void* entry,
+                                const char* lineStr) {
+  auto element = static_cast<PlotSeries*>(entry);
+  wpi::StringRef line{lineStr};
+  auto [name, value] = line.split('=');
+  name = name.trim();
+  value = value.trim();
+  element->ReadIni(name, value);
+}
+
+static void PlotSeries_WriteAll(ImGuiContext* ctx,
+                                ImGuiSettingsHandler* handler,
+                                ImGuiTextBuffer* out_buf) {
+  for (size_t i = 0; i < gPlots.size(); ++i) {
+    for (const auto& series : gPlots[i].m_series) {
+      out_buf->appendf("[PlotSeries][%d,%s]\n", static_cast<int>(i),
+                       series->GetId().c_str());
+      series->WriteIni(out_buf);
+      out_buf->append("\n");
+    }
+  }
+}
+
+void PlotGui::Initialize() {
+  gPlots.Initialize();
+
+  // hook ini handler for PlotSeries to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = "PlotSeries";
+  iniHandler.TypeHash = ImHashStr("PlotSeries");
+  iniHandler.ReadOpenFn = PlotSeries_ReadOpen;
+  iniHandler.ReadLineFn = PlotSeries_ReadLine;
+  iniHandler.WriteAllFn = PlotSeries_WriteAll;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+
+  // HALSimGui::AddExecute([] { ImPlot::ShowDemoWindow(); });
+  HALSimGui::AddWindow("Plot", DisplayPlot);
+  HALSimGui::SetDefaultWindowPos("Plot", 600, 75);
+  HALSimGui::SetDefaultWindowSize("Plot", 300, 200);
+
+  HALSimGui::AddWindow("Plot Settings", DisplayPlotSettings);
+  HALSimGui::SetDefaultWindowPos("Plot Settings", 902, 75);
+  HALSimGui::SetDefaultWindowSize("Plot Settings", 120, 200);
+}

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/PlotGui.h b/simulation/halsim_gui/src/main/native/cpp/PlotGui.h
new file mode 100644
index 0000000..6bbd337
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/cpp/PlotGui.h
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+class PlotGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/RelayGui.cpp b/simulation/halsim_gui/src/main/native/cpp/RelayGui.cpp
index a833fff..9171678 100644
--- a/simulation/halsim_gui/src/main/native/cpp/RelayGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/RelayGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,25 +9,63 @@
 
 #include <cstdio>
 #include <cstring>
+#include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/RelayData.h>
 #include <imgui.h>
-#include <mockdata/RelayData.h>
 
 #include "ExtraGuiWidgets.h"
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(RelayForward, "RelayFwd");
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(RelayReverse, "RelayRev");
+}  // namespace
+
+static IniSaver<NameInfo> gRelays{"Relay"};
+static std::vector<std::unique_ptr<RelayForwardSource>> gRelayForwardSources;
+static std::vector<std::unique_ptr<RelayReverseSource>> gRelayReverseSources;
+
+static void UpdateRelaySources() {
+  for (int i = 0, iend = gRelayForwardSources.size(); i < iend; ++i) {
+    auto& source = gRelayForwardSources[i];
+    if (HALSIM_GetRelayInitializedForward(i)) {
+      if (!source) {
+        source = std::make_unique<RelayForwardSource>(i);
+        source->SetName(gRelays[i].GetName());
+      }
+    } else {
+      source.reset();
+    }
+  }
+  for (int i = 0, iend = gRelayReverseSources.size(); i < iend; ++i) {
+    auto& source = gRelayReverseSources[i];
+    if (HALSIM_GetRelayInitializedReverse(i)) {
+      if (!source) {
+        source = std::make_unique<RelayReverseSource>(i);
+        source->SetName(gRelays[i].GetName());
+      }
+    } else {
+      source.reset();
+    }
+  }
+}
+
 static void DisplayRelays() {
   bool hasOutputs = false;
   bool first = true;
-  static const int numRelay = HAL_GetNumRelayHeaders();
-  for (int i = 0; i < numRelay; ++i) {
-    bool forwardInit = HALSIM_GetRelayInitializedForward(i);
-    bool reverseInit = HALSIM_GetRelayInitializedReverse(i);
+  for (int i = 0, iend = gRelayForwardSources.size(); i < iend; ++i) {
+    auto forwardSource = gRelayForwardSources[i].get();
+    auto reverseSource = gRelayReverseSources[i].get();
 
-    if (forwardInit || reverseInit) {
+    if (forwardSource || reverseSource) {
       hasOutputs = true;
 
       if (!first)
@@ -38,26 +76,44 @@
       bool forward = false;
       bool reverse = false;
       if (!HALSimGui::AreOutputsDisabled()) {
-        reverse = HALSIM_GetRelayReverse(i);
-        forward = HALSIM_GetRelayForward(i);
+        if (forwardSource) forward = forwardSource->GetValue();
+        if (reverseSource) reverse = reverseSource->GetValue();
       }
 
-      ImGui::Text("Relay[%d]", i);
+      auto& info = gRelays[i];
+      info.PushEditNameId(i);
+      if (info.HasName())
+        ImGui::Text("%s [%d]", info.GetName(), i);
+      else
+        ImGui::Text("Relay[%d]", i);
+      ImGui::PopID();
+      if (info.PopupEditName(i)) {
+        if (forwardSource) forwardSource->SetName(info.GetName());
+        if (reverseSource) reverseSource->SetName(info.GetName());
+      }
       ImGui::SameLine();
 
       // show forward and reverse as LED indicators
       static const ImU32 colors[] = {IM_COL32(255, 255, 102, 255),
                                      IM_COL32(255, 0, 0, 255),
                                      IM_COL32(128, 128, 128, 255)};
-      int values[2] = {reverseInit ? (reverse ? 2 : -2) : -3,
-                       forwardInit ? (forward ? 1 : -1) : -3};
-      DrawLEDs(values, 2, 2, colors);
+      int values[2] = {reverseSource ? (reverse ? 2 : -2) : -3,
+                       forwardSource ? (forward ? 1 : -1) : -3};
+      GuiDataSource* sources[2] = {reverseSource, forwardSource};
+      ImGui::PushID(i);
+      DrawLEDSources(values, sources, 2, 2, colors);
+      ImGui::PopID();
     }
   }
   if (!hasOutputs) ImGui::Text("No relays");
 }
 
 void RelayGui::Initialize() {
+  gRelays.Initialize();
+  int numRelays = HAL_GetNumRelayHeaders();
+  gRelayForwardSources.resize(numRelays);
+  gRelayReverseSources.resize(numRelays);
+  HALSimGui::AddExecute(UpdateRelaySources);
   HALSimGui::AddWindow("Relays", DisplayRelays,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("Relays", 180, 20);

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.cpp b/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.cpp
index e4d6650..b4f8909 100644
--- a/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,111 +7,153 @@
 
 #include "RoboRioGui.h"
 
-#include <imgui.h>
-#include <mockdata/RoboRioData.h>
+#include <memory>
 
+#include <hal/simulation/RoboRioData.h>
+#include <imgui.h>
+
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioVInVoltage, "Rio Input Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioVInCurrent, "Rio Input Current");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage6V, "Rio 6V Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent6V, "Rio 6V Current");
+HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive6V, "Rio 6V Active");
+HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults6V, "Rio 6V Faults");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage5V, "Rio 5V Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent5V, "Rio 5V Current");
+HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive5V, "Rio 5V Active");
+HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults5V, "Rio 5V Faults");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage3V3, "Rio 3.3V Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent3V3, "Rio 3.3V Current");
+HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive3V3, "Rio 3.3V Active");
+HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults3V3, "Rio 3.3V Faults");
+struct RoboRioSource {
+  RoboRioVInVoltageSource vInVoltage;
+  RoboRioVInCurrentSource vInCurrent;
+  RoboRioUserVoltage6VSource userVoltage6V;
+  RoboRioUserCurrent6VSource userCurrent6V;
+  RoboRioUserActive6VSource userActive6V;
+  RoboRioUserFaults6VSource userFaults6V;
+  RoboRioUserVoltage5VSource userVoltage5V;
+  RoboRioUserCurrent5VSource userCurrent5V;
+  RoboRioUserActive5VSource userActive5V;
+  RoboRioUserFaults5VSource userFaults5V;
+  RoboRioUserVoltage3V3Source userVoltage3V3;
+  RoboRioUserCurrent3V3Source userCurrent3V3;
+  RoboRioUserActive3V3Source userActive3V3;
+  RoboRioUserFaults3V3Source userFaults3V3;
+};
+}  // namespace
+
+static std::unique_ptr<RoboRioSource> gRioSource;
+
+static void UpdateRoboRioSources() {
+  if (!gRioSource) gRioSource = std::make_unique<RoboRioSource>();
+}
+
 static void DisplayRoboRio() {
   ImGui::Button("User Button");
-  HALSIM_SetRoboRioFPGAButton(0, ImGui::IsItemActive());
+  HALSIM_SetRoboRioFPGAButton(ImGui::IsItemActive());
 
   ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
 
   if (ImGui::CollapsingHeader("RoboRIO Input")) {
     {
-      double val = HALSIM_GetRoboRioVInVoltage(0);
-      if (ImGui::InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioVInVoltage(0, val);
+      double val = gRioSource->vInVoltage.GetValue();
+      if (gRioSource->vInVoltage.InputDouble("Voltage (V)", &val))
+        HALSIM_SetRoboRioVInVoltage(val);
     }
 
     {
-      double val = HALSIM_GetRoboRioVInCurrent(0);
-      if (ImGui::InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioVInCurrent(0, val);
+      double val = gRioSource->vInCurrent.GetValue();
+      if (gRioSource->vInCurrent.InputDouble("Current (A)", &val))
+        HALSIM_SetRoboRioVInCurrent(val);
     }
   }
 
   if (ImGui::CollapsingHeader("6V Rail")) {
     {
-      double val = HALSIM_GetRoboRioUserVoltage6V(0);
-      if (ImGui::InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioUserVoltage6V(0, val);
+      double val = gRioSource->userVoltage6V.GetValue();
+      if (gRioSource->userVoltage6V.InputDouble("Voltage (V)", &val))
+        HALSIM_SetRoboRioUserVoltage6V(val);
     }
 
     {
-      double val = HALSIM_GetRoboRioUserCurrent6V(0);
-      if (ImGui::InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioUserCurrent6V(0, val);
+      double val = gRioSource->userCurrent6V.GetValue();
+      if (gRioSource->userCurrent6V.InputDouble("Current (A)", &val))
+        HALSIM_SetRoboRioUserCurrent6V(val);
     }
 
     {
       static const char* options[] = {"inactive", "active"};
-      int val = HALSIM_GetRoboRioUserActive6V(0) ? 1 : 0;
-      if (ImGui::Combo("Active", &val, options, 2))
-        HALSIM_SetRoboRioUserActive6V(0, val);
+      int val = gRioSource->userActive6V.GetValue() ? 1 : 0;
+      if (gRioSource->userActive6V.Combo("Active", &val, options, 2))
+        HALSIM_SetRoboRioUserActive6V(val);
     }
 
     {
-      int val = HALSIM_GetRoboRioUserFaults6V(0);
-      if (ImGui::InputInt("Faults", &val))
-        HALSIM_SetRoboRioUserFaults6V(0, val);
+      int val = gRioSource->userFaults6V.GetValue();
+      if (gRioSource->userFaults6V.InputInt("Faults", &val))
+        HALSIM_SetRoboRioUserFaults6V(val);
     }
   }
 
   if (ImGui::CollapsingHeader("5V Rail")) {
     {
-      double val = HALSIM_GetRoboRioUserVoltage5V(0);
-      if (ImGui::InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioUserVoltage5V(0, val);
+      double val = gRioSource->userVoltage5V.GetValue();
+      if (gRioSource->userVoltage5V.InputDouble("Voltage (V)", &val))
+        HALSIM_SetRoboRioUserVoltage5V(val);
     }
 
     {
-      double val = HALSIM_GetRoboRioUserCurrent5V(0);
-      if (ImGui::InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioUserCurrent5V(0, val);
+      double val = gRioSource->userCurrent5V.GetValue();
+      if (gRioSource->userCurrent5V.InputDouble("Current (A)", &val))
+        HALSIM_SetRoboRioUserCurrent5V(val);
     }
 
     {
       static const char* options[] = {"inactive", "active"};
-      int val = HALSIM_GetRoboRioUserActive5V(0) ? 1 : 0;
-      if (ImGui::Combo("Active", &val, options, 2))
-        HALSIM_SetRoboRioUserActive5V(0, val);
+      int val = gRioSource->userActive5V.GetValue() ? 1 : 0;
+      if (gRioSource->userActive5V.Combo("Active", &val, options, 2))
+        HALSIM_SetRoboRioUserActive5V(val);
     }
 
     {
-      int val = HALSIM_GetRoboRioUserFaults5V(0);
-      if (ImGui::InputInt("Faults", &val))
-        HALSIM_SetRoboRioUserFaults5V(0, val);
+      int val = gRioSource->userFaults5V.GetValue();
+      if (gRioSource->userFaults5V.InputInt("Faults", &val))
+        HALSIM_SetRoboRioUserFaults5V(val);
     }
   }
 
   if (ImGui::CollapsingHeader("3.3V Rail")) {
     {
-      double val = HALSIM_GetRoboRioUserVoltage3V3(0);
-      if (ImGui::InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioUserVoltage3V3(0, val);
+      double val = gRioSource->userVoltage3V3.GetValue();
+      if (gRioSource->userVoltage3V3.InputDouble("Voltage (V)", &val))
+        HALSIM_SetRoboRioUserVoltage3V3(val);
     }
 
     {
-      double val = HALSIM_GetRoboRioUserCurrent3V3(0);
-      if (ImGui::InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioUserCurrent3V3(0, val);
+      double val = gRioSource->userCurrent3V3.GetValue();
+      if (gRioSource->userCurrent3V3.InputDouble("Current (A)", &val))
+        HALSIM_SetRoboRioUserCurrent3V3(val);
     }
 
     {
       static const char* options[] = {"inactive", "active"};
-      int val = HALSIM_GetRoboRioUserActive3V3(0) ? 1 : 0;
-      if (ImGui::Combo("Active", &val, options, 2))
-        HALSIM_SetRoboRioUserActive3V3(0, val);
+      int val = HALSIM_GetRoboRioUserActive3V3() ? 1 : 0;
+      if (gRioSource->userActive3V3.Combo("Active", &val, options, 2))
+        HALSIM_SetRoboRioUserActive3V3(val);
     }
 
     {
-      int val = HALSIM_GetRoboRioUserFaults3V3(0);
-      if (ImGui::InputInt("Faults", &val))
-        HALSIM_SetRoboRioUserFaults3V3(0, val);
+      int val = gRioSource->userFaults3V3.GetValue();
+      if (gRioSource->userFaults3V3.InputInt("Faults", &val))
+        HALSIM_SetRoboRioUserFaults3V3(val);
     }
   }
 
@@ -119,6 +161,7 @@
 }
 
 void RoboRioGui::Initialize() {
+  HALSimGui::AddExecute(UpdateRoboRioSources);
   HALSimGui::AddWindow("RoboRIO", DisplayRoboRio,
                        ImGuiWindowFlags_AlwaysAutoResize);
   // hide it by default

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp b/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp
index 2063007..2802000 100644
--- a/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,54 +9,85 @@
 
 #include <stdint.h>
 
+#include <functional>
+#include <memory>
 #include <vector>
 
 #include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
 #include <imgui.h>
-#include <imgui_internal.h>
-#include <mockdata/SimDeviceData.h>
-#include <wpi/StringMap.h>
+#include <wpi/DenseMap.h>
 
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaverInfo.h"
+#include "IniSaverString.h"
 
 using namespace halsimgui;
 
 namespace {
-struct ElementInfo {
-  bool open = false;
-  bool visible = true;
+
+struct ElementInfo : public NameInfo, public OpenInfo {
+  bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
+    if (NameInfo::ReadIni(name, value)) return true;
+    if (OpenInfo::ReadIni(name, value)) return true;
+    return false;
+  }
+  void WriteIni(ImGuiTextBuffer* out) {
+    NameInfo::WriteIni(out);
+    OpenInfo::WriteIni(out);
+  }
+  bool visible = true;  // not saved
 };
+
+class SimValueSource : public GuiDataSource {
+ public:
+  explicit SimValueSource(HAL_SimValueHandle handle, const char* device,
+                          const char* name)
+      : GuiDataSource(wpi::Twine{device} + wpi::Twine{'-'} + name),
+        m_callback{HALSIM_RegisterSimValueChangedCallback(
+            handle, this, CallbackFunc, true)} {}
+  ~SimValueSource() {
+    if (m_callback != 0) HALSIM_CancelSimValueChangedCallback(m_callback);
+  }
+
+ private:
+  static void CallbackFunc(const char*, void* param, HAL_SimValueHandle,
+                           HAL_Bool, const HAL_Value* value) {
+    auto source = static_cast<SimValueSource*>(param);
+    if (value->type == HAL_BOOLEAN) {
+      source->SetValue(value->data.v_boolean);
+      source->SetDigital(true);
+    } else if (value->type == HAL_DOUBLE) {
+      source->SetValue(value->data.v_double);
+      source->SetDigital(false);
+    }
+  }
+
+  int32_t m_callback;
+};
+
 }  // namespace
 
 static std::vector<std::function<void()>> gDeviceExecutors;
-static wpi::StringMap<ElementInfo> gElements;
+static IniSaverString<ElementInfo> gElements{"Device"};
+static wpi::DenseMap<HAL_SimValueHandle, std::unique_ptr<SimValueSource>>
+    gSimValueSources;
 
-// read/write open state to ini file
-static void* DevicesReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             const char* name) {
-  return &gElements[name];
-}
-
-static void DevicesReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                            void* entry, const char* lineStr) {
-  ElementInfo* element = static_cast<ElementInfo*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  if (name == "open") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    element->open = num;
-  }
-}
-
-static void DevicesWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                            ImGuiTextBuffer* out_buf) {
-  for (auto&& entry : gElements) {
-    out_buf->appendf("[Device][%s]\nopen=%d\n\n", entry.getKey().data(),
-                     entry.getValue().open ? 1 : 0);
-  }
+static void UpdateSimValueSources() {
+  HALSIM_EnumerateSimDevices(
+      "", nullptr, [](const char* name, void*, HAL_SimDeviceHandle handle) {
+        HALSIM_EnumerateSimValues(
+            handle, const_cast<char*>(name),
+            [](const char* name, void* deviceV, HAL_SimValueHandle handle,
+               HAL_Bool readonly, const HAL_Value* value) {
+              auto device = static_cast<const char*>(deviceV);
+              auto& source = gSimValueSources[handle];
+              if (!source) {
+                source = std::make_unique<SimValueSource>(handle, device, name);
+              }
+            });
+      });
 }
 
 void SimDeviceGui::Hide(const char* name) { gElements[name].visible = false; }
@@ -69,20 +100,22 @@
   auto& element = gElements[label];
   if (!element.visible) return false;
 
-  if (ImGui::CollapsingHeader(
-          label, flags | (element.open ? ImGuiTreeNodeFlags_DefaultOpen : 0))) {
-    ImGui::PushID(label);
-    element.open = true;
-    return true;
-  }
-  element.open = false;
-  return false;
+  char name[128];
+  element.GetLabel(name, sizeof(name), label);
+
+  bool open = ImGui::CollapsingHeader(
+      name, flags | (element.IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0));
+  element.SetOpen(open);
+  element.PopupEditName(label);
+
+  if (open) ImGui::PushID(label);
+  return open;
 }
 
 void SimDeviceGui::FinishDevice() { ImGui::PopID(); }
 
-bool DisplayValueImpl(const char* name, bool readonly, HAL_Value* value,
-                      const char** options, int32_t numOptions) {
+static bool DisplayValueImpl(const char* name, bool readonly, HAL_Value* value,
+                             const char** options, int32_t numOptions) {
   // read-only
   if (readonly) {
     switch (value->type) {
@@ -159,11 +192,36 @@
   return false;
 }
 
+static bool DisplayValueSourceImpl(const char* name, bool readonly,
+                                   HAL_Value* value,
+                                   const GuiDataSource* source,
+                                   const char** options, int32_t numOptions) {
+  if (!source)
+    return DisplayValueImpl(name, readonly, value, options, numOptions);
+  ImGui::PushID(name);
+  bool rv = DisplayValueImpl("", readonly, value, options, numOptions);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(name);
+  source->EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
 bool SimDeviceGui::DisplayValue(const char* name, bool readonly,
                                 HAL_Value* value, const char** options,
                                 int32_t numOptions) {
+  return DisplayValueSource(name, readonly, value, nullptr, options,
+                            numOptions);
+}
+
+bool SimDeviceGui::DisplayValueSource(const char* name, bool readonly,
+                                      HAL_Value* value,
+                                      const GuiDataSource* source,
+                                      const char** options,
+                                      int32_t numOptions) {
   ImGui::SetNextItemWidth(ImGui::GetWindowWidth() * 0.5f);
-  return DisplayValueImpl(name, readonly, value, options, numOptions);
+  return DisplayValueSourceImpl(name, readonly, value, source, options,
+                                numOptions);
 }
 
 static void SimDeviceDisplayValue(const char* name, void*,
@@ -176,7 +234,9 @@
     options = HALSIM_GetSimValueEnumOptions(handle, &numOptions);
 
   HAL_Value valueCopy = *value;
-  if (DisplayValueImpl(name, readonly, &valueCopy, options, numOptions))
+  if (DisplayValueSourceImpl(name, readonly, &valueCopy,
+                             gSimValueSources[handle].get(), options,
+                             numOptions))
     HAL_SetSimValue(handle, valueCopy);
 }
 
@@ -201,15 +261,8 @@
 }
 
 void SimDeviceGui::Initialize() {
-  // hook ini handler to save device settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "Device";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = DevicesReadOpen;
-  iniHandler.ReadLineFn = DevicesReadLine;
-  iniHandler.WriteAllFn = DevicesWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
+  gElements.Initialize();
+  HALSimGui::AddExecute(UpdateSimValueSources);
   HALSimGui::AddWindow("Other Devices", DisplayDeviceTree);
   HALSimGui::SetDefaultWindowPos("Other Devices", 1025, 20);
   HALSimGui::SetDefaultWindowSize("Other Devices", 250, 695);

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.cpp b/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.cpp
index 75712c6..b0f802e 100644
--- a/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,30 +9,69 @@
 
 #include <cstdio>
 #include <cstring>
+#include <memory>
+#include <vector>
 
 #include <hal/Ports.h>
+#include <hal/simulation/PCMData.h>
 #include <imgui.h>
-#include <mockdata/PCMData.h>
 #include <wpi/SmallVector.h>
 
 #include "ExtraGuiWidgets.h"
+#include "GuiDataSource.h"
 #include "HALSimGui.h"
+#include "IniSaver.h"
+#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED2(PCMSolenoidOutput, "Solenoid");
+struct PCMSource {
+  explicit PCMSource(int numChannels) : solenoids(numChannels) {}
+  std::vector<std::unique_ptr<PCMSolenoidOutputSource>> solenoids;
+  int initCount = 0;
+};
+}  // namespace
+
+static IniSaver<OpenInfo> gPCMs{"PCM"};
+static IniSaver<NameInfo> gSolenoids{"Solenoid"};
+static std::vector<PCMSource> gPCMSources;
+
+static void UpdateSolenoidSources() {
+  for (int i = 0, iend = gPCMSources.size(); i < iend; ++i) {
+    auto& pcmSource = gPCMSources[i];
+    int numChannels = pcmSource.solenoids.size();
+    pcmSource.initCount = 0;
+    for (int j = 0; j < numChannels; ++j) {
+      auto& source = pcmSource.solenoids[j];
+      if (HALSIM_GetPCMSolenoidInitialized(i, j)) {
+        if (!source) {
+          source = std::make_unique<PCMSolenoidOutputSource>(i, j);
+          source->SetName(gSolenoids[i * numChannels + j].GetName());
+        }
+        ++pcmSource.initCount;
+      } else {
+        source.reset();
+      }
+    }
+  }
+}
+
 static void DisplaySolenoids() {
   bool hasOutputs = false;
-  static const int numPCM = HAL_GetNumPCMModules();
-  static const int numChannels = HAL_GetNumSolenoidChannels();
-  for (int i = 0; i < numPCM; ++i) {
-    bool anyInit = false;
+  for (int i = 0, iend = gPCMSources.size(); i < iend; ++i) {
+    auto& pcmSource = gPCMSources[i];
+    if (pcmSource.initCount == 0) continue;
+    hasOutputs = true;
+
+    int numChannels = pcmSource.solenoids.size();
     wpi::SmallVector<int, 16> channels;
     channels.resize(numChannels);
     for (int j = 0; j < numChannels; ++j) {
-      if (HALSIM_GetPCMSolenoidInitialized(i, j)) {
-        anyInit = true;
+      if (pcmSource.solenoids[j]) {
         channels[j] = (!HALSimGui::AreOutputsDisabled() &&
-                       HALSIM_GetPCMSolenoidOutput(i, j))
+                       pcmSource.solenoids[j]->GetValue())
                           ? 1
                           : -1;
       } else {
@@ -40,21 +79,51 @@
       }
     }
 
-    if (!anyInit) continue;
-    hasOutputs = true;
-
-    ImGui::Text("PCM[%d]", i);
+    char name[128];
+    std::snprintf(name, sizeof(name), "PCM[%d]", i);
+    auto& pcmInfo = gPCMs[i];
+    bool open = ImGui::CollapsingHeader(
+        name, pcmInfo.IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
+    pcmInfo.SetOpen(open);
+    ImGui::SetItemAllowOverlap();
     ImGui::SameLine();
 
     // show channels as LED indicators
     static const ImU32 colors[] = {IM_COL32(255, 255, 102, 255),
                                    IM_COL32(128, 128, 128, 255)};
     DrawLEDs(channels.data(), channels.size(), channels.size(), colors);
+
+    if (open) {
+      ImGui::PushID(i);
+      ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
+      for (int j = 0; j < numChannels; ++j) {
+        if (!pcmSource.solenoids[j]) continue;
+        auto& info = gSolenoids[i * numChannels + j];
+        info.GetLabel(name, sizeof(name), "Solenoid", j);
+        ImGui::PushID(j);
+        pcmSource.solenoids[j]->LabelText(name, "%s",
+                                          channels[j] == 1 ? "On" : "Off");
+        if (info.PopupEditName(j)) {
+          pcmSource.solenoids[j]->SetName(info.GetName());
+        }
+        ImGui::PopID();
+      }
+      ImGui::PopItemWidth();
+      ImGui::PopID();
+    }
   }
   if (!hasOutputs) ImGui::Text("No solenoids");
 }
 
 void SolenoidGui::Initialize() {
+  gPCMs.Initialize();
+  gSolenoids.Initialize();
+  const int numModules = HAL_GetNumPCMModules();
+  const int numChannels = HAL_GetNumSolenoidChannels();
+  gPCMSources.reserve(numModules);
+  for (int i = 0; i < numModules; ++i) gPCMSources.emplace_back(numChannels);
+
+  HALSimGui::AddExecute(UpdateSolenoidSources);
   HALSimGui::AddWindow("Solenoids", DisplaySolenoids,
                        ImGuiWindowFlags_AlwaysAutoResize);
   HALSimGui::SetDefaultWindowPos("Solenoids", 290, 20);

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp b/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
index 9897a7a..18c76b0 100644
--- a/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,9 +12,9 @@
 #include <vector>
 
 #include <hal/HALBase.h>
+#include <hal/simulation/MockHooks.h>
+#include <hal/simulation/NotifierData.h>
 #include <imgui.h>
-#include <mockdata/MockHooks.h>
-#include <mockdata/NotifierData.h>
 
 #include "HALSimGui.h"
 
@@ -32,7 +32,8 @@
   if (ImGui::Button("Step")) {
     HALSIM_PauseTiming();
     uint64_t nextTimeout = HALSIM_GetNextNotifierTimeout();
-    if (nextTimeout != UINT64_MAX) HALSIM_StepTiming(nextTimeout - curTime);
+    if (nextTimeout != UINT64_MAX)
+      HALSIM_StepTimingAsync(nextTimeout - curTime);
   }
   ImGui::PopButtonRepeat();
   ImGui::PushItemWidth(ImGui::GetFontSize() * 4);

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

diff --git a/simulation/halsim_gui/src/main/native/cpp/main.cpp b/simulation/halsim_gui/src/main/native/cpp/main.cpp
index d5e0b16..a904580 100644
--- a/simulation/halsim_gui/src/main/native/cpp/main.cpp
+++ b/simulation/halsim_gui/src/main/native/cpp/main.cpp
@@ -1,11 +1,13 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <hal/Extensions.h>
 #include <hal/Main.h>
+#include <wpi/StringRef.h>
 #include <wpi/raw_ostream.h>
 
 #include "AccelerometerGui.h"
@@ -17,9 +19,13 @@
 #include "DIOGui.h"
 #include "DriverStationGui.h"
 #include "EncoderGui.h"
+#include "Field2D.h"
 #include "HALSimGui.h"
+#include "Mechanism2D.h"
+#include "NetworkTablesGui.h"
 #include "PDPGui.h"
 #include "PWMGui.h"
+#include "PlotGui.h"
 #include "RelayGui.h"
 #include "RoboRioGui.h"
 #include "SimDeviceGui.h"
@@ -33,6 +39,7 @@
 __declspec(dllexport)
 #endif
     int HALSIM_InitExtension(void) {
+  HALSimGui::GlobalInit();
   HALSimGui::Add(AccelerometerGui::Initialize);
   HALSimGui::Add(AddressableLEDGui::Initialize);
   HALSimGui::Add(AnalogGyroGui::Initialize);
@@ -42,7 +49,11 @@
   HALSimGui::Add(DriverStationGui::Initialize);
   HALSimGui::Add(DIOGui::Initialize);
   HALSimGui::Add(EncoderGui::Initialize);
+  HALSimGui::Add(Field2D::Initialize);
+  HALSimGui::Add(Mechanism2D::Initialize);
+  HALSimGui::Add(NetworkTablesGui::Initialize);
   HALSimGui::Add(PDPGui::Initialize);
+  HALSimGui::Add(PlotGui::Initialize);
   HALSimGui::Add(PWMGui::Initialize);
   HALSimGui::Add(RelayGui::Initialize);
   HALSimGui::Add(RoboRioGui::Initialize);
@@ -52,6 +63,12 @@
 
   wpi::outs() << "Simulator GUI Initializing.\n";
   if (!HALSimGui::Initialize()) return 0;
+  HAL_RegisterExtensionListener(
+      nullptr, [](void*, const char* name, void* data) {
+        if (wpi::StringRef{name} == "ds_socket") {
+          DriverStationGui::SetDSSocketExtension(data);
+        }
+      });
   HAL_SetMain(nullptr, HALSimGui::Main, HALSimGui::Exit);
   wpi::outs() << "Simulator GUI Initialized!\n";
 

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

diff --git a/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h b/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
index 7374cac..91d7301 100644
--- a/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
+++ b/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,8 @@
 
 namespace halsimgui {
 
+class GuiDataSource;
+
 /**
  * DrawLEDs() configuration for 2D arrays.
  */
@@ -60,4 +62,28 @@
               float size = 0.0f, float spacing = 0.0f,
               const LEDConfig& config = LEDConfig{});
 
+/**
+ * Draw a 2D array of LEDs.
+ *
+ * Values are indices into colors array.  Positive values are filled (lit),
+ * negative values are unfilled (dark / border only).  The actual color index
+ * is the absolute value of the value - 1.  0 values are not drawn at all
+ * (an empty space is left).
+ *
+ * @param values values array
+ * @param sources sources array
+ * @param numValues size of values and sources arrays
+ * @param cols number of columns
+ * @param colors colors array
+ * @param size size of each LED (both horizontal and vertical);
+ *             if 0, defaults to 1/2 of font size
+ * @param spacing spacing between each LED (both horizontal and vertical);
+ *                if 0, defaults to 1/3 of font size
+ * @param config 2D array configuration
+ */
+void DrawLEDSources(const int* values, GuiDataSource** sources, int numValues,
+                    int cols, const ImU32* colors, float size = 0.0f,
+                    float spacing = 0.0f,
+                    const LEDConfig& config = LEDConfig{});
+
 }  // namespace halsimgui

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

diff --git a/simulation/halsim_gui/src/main/native/include/GuiDataSource.h b/simulation/halsim_gui/src/main/native/include/GuiDataSource.h
new file mode 100644
index 0000000..117d6ea
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/include/GuiDataSource.h
@@ -0,0 +1,186 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <string>
+
+#include <imgui.h>
+#include <wpi/Signal.h>
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+#include <wpi/spinlock.h>
+
+namespace halsimgui {
+
+/**
+ * A data source.
+ */
+class GuiDataSource {
+ public:
+  explicit GuiDataSource(const wpi::Twine& id);
+  GuiDataSource(const wpi::Twine& id, int index);
+  GuiDataSource(const wpi::Twine& id, int index, int index2);
+  ~GuiDataSource();
+
+  GuiDataSource(const GuiDataSource&) = delete;
+  GuiDataSource& operator=(const GuiDataSource&) = delete;
+
+  const char* GetId() const { return m_id.c_str(); }
+
+  void SetName(const wpi::Twine& name) { m_name = name.str(); }
+  const char* GetName() const { return m_name.c_str(); }
+
+  void SetDigital(bool digital) { m_digital = digital; }
+  bool IsDigital() const { return m_digital; }
+
+  void SetValue(double value) {
+    m_value = value;
+    valueChanged(value);
+  }
+  double GetValue() const { return m_value; }
+
+  // drag source helpers
+  void LabelText(const char* label, const char* fmt, ...) const;
+  void LabelTextV(const char* label, const char* fmt, va_list args) const;
+  bool Combo(const char* label, int* current_item, const char* const items[],
+             int items_count, int popup_max_height_in_items = -1) const;
+  bool SliderFloat(const char* label, float* v, float v_min, float v_max,
+                   const char* format = "%.3f", float power = 1.0f) const;
+  bool InputDouble(const char* label, double* v, double step = 0.0,
+                   double step_fast = 0.0, const char* format = "%.6f",
+                   ImGuiInputTextFlags flags = 0) const;
+  bool InputInt(const char* label, int* v, int step = 1, int step_fast = 100,
+                ImGuiInputTextFlags flags = 0) const;
+  void EmitDrag(ImGuiDragDropFlags flags = 0) const;
+
+  wpi::sig::SignalBase<wpi::spinlock, double> valueChanged;
+
+  static GuiDataSource* Find(wpi::StringRef id);
+
+  static wpi::sig::Signal<const char*, GuiDataSource*> sourceCreated;
+
+ private:
+  std::string m_id;
+  std::string m_name;
+  bool m_digital = false;
+  std::atomic<double> m_value = 0;
+};
+
+}  // namespace halsimgui
+
+#define HALSIMGUI_DATASOURCE(cbname, id, TYPE, vtype)                         \
+  class cbname##Source : public ::halsimgui::GuiDataSource {                  \
+   public:                                                                    \
+    cbname##Source()                                                          \
+        : GuiDataSource(id),                                                  \
+          m_callback{                                                         \
+              HALSIM_Register##cbname##Callback(CallbackFunc, this, true)} {  \
+      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
+    }                                                                         \
+                                                                              \
+    ~cbname##Source() {                                                       \
+      if (m_callback != 0) HALSIM_Cancel##cbname##Callback(m_callback);       \
+    }                                                                         \
+                                                                              \
+   private:                                                                   \
+    static void CallbackFunc(const char*, void* param,                        \
+                             const HAL_Value* value) {                        \
+      if (value->type == HAL_##TYPE)                                          \
+        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
+    }                                                                         \
+                                                                              \
+    int32_t m_callback;                                                       \
+  }
+
+#define HALSIMGUI_DATASOURCE_BOOLEAN(cbname, id) \
+  HALSIMGUI_DATASOURCE(cbname, id, BOOLEAN, boolean)
+
+#define HALSIMGUI_DATASOURCE_DOUBLE(cbname, id) \
+  HALSIMGUI_DATASOURCE(cbname, id, DOUBLE, double)
+
+#define HALSIMGUI_DATASOURCE_INT(cbname, id) \
+  HALSIMGUI_DATASOURCE(cbname, id, INT, int)
+
+#define HALSIMGUI_DATASOURCE_INDEXED(cbname, id, TYPE, vtype)                 \
+  class cbname##Source : public ::halsimgui::GuiDataSource {                  \
+   public:                                                                    \
+    explicit cbname##Source(int32_t index, int channel = -1)                  \
+        : GuiDataSource(id, channel < 0 ? index : channel),                   \
+          m_index{index},                                                     \
+          m_channel{channel < 0 ? index : channel},                           \
+          m_callback{HALSIM_Register##cbname##Callback(index, CallbackFunc,   \
+                                                       this, true)} {         \
+      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
+    }                                                                         \
+                                                                              \
+    ~cbname##Source() {                                                       \
+      if (m_callback != 0)                                                    \
+        HALSIM_Cancel##cbname##Callback(m_index, m_callback);                 \
+    }                                                                         \
+                                                                              \
+    int32_t GetIndex() const { return m_index; }                              \
+                                                                              \
+    int GetChannel() const { return m_channel; }                              \
+                                                                              \
+   private:                                                                   \
+    static void CallbackFunc(const char*, void* param,                        \
+                             const HAL_Value* value) {                        \
+      if (value->type == HAL_##TYPE)                                          \
+        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
+    }                                                                         \
+                                                                              \
+    int32_t m_index;                                                          \
+    int m_channel;                                                            \
+    int32_t m_callback;                                                       \
+  }
+
+#define HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED(cbname, id, BOOLEAN, boolean)
+
+#define HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED(cbname, id, DOUBLE, double)
+
+#define HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, TYPE, vtype)                \
+  class cbname##Source : public ::halsimgui::GuiDataSource {                  \
+   public:                                                                    \
+    explicit cbname##Source(int32_t index, int32_t channel)                   \
+        : GuiDataSource(id, index, channel),                                  \
+          m_index{index},                                                     \
+          m_channel{channel},                                                 \
+          m_callback{HALSIM_Register##cbname##Callback(                       \
+              index, channel, CallbackFunc, this, true)} {                    \
+      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
+    }                                                                         \
+                                                                              \
+    ~cbname##Source() {                                                       \
+      if (m_callback != 0)                                                    \
+        HALSIM_Cancel##cbname##Callback(m_index, m_channel, m_callback);      \
+    }                                                                         \
+                                                                              \
+    int32_t GetIndex() const { return m_index; }                              \
+                                                                              \
+    int32_t GetChannel() const { return m_channel; }                          \
+                                                                              \
+   private:                                                                   \
+    static void CallbackFunc(const char*, void* param,                        \
+                             const HAL_Value* value) {                        \
+      if (value->type == HAL_##TYPE)                                          \
+        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
+    }                                                                         \
+                                                                              \
+    int32_t m_index;                                                          \
+    int32_t m_channel;                                                        \
+    int32_t m_callback;                                                       \
+  }
+
+#define HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED2(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, BOOLEAN, boolean)
+
+#define HALSIMGUI_DATASOURCE_DOUBLE_INDEXED2(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, DOUBLE, double)

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

diff --git a/simulation/halsim_gui/src/main/native/include/HALSimGui.h b/simulation/halsim_gui/src/main/native/include/HALSimGui.h
index fd71cbe..4a4c912 100644
--- a/simulation/halsim_gui/src/main/native/include/HALSimGui.h
+++ b/simulation/halsim_gui/src/main/native/include/HALSimGui.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,6 +23,7 @@
 void HALSIMGUI_SetDefaultWindowPos(const char* name, float x, float y);
 void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
                                     float height);
+void HALSIMGUI_SetWindowPadding(const char* name, float x, float y);
 int HALSIMGUI_AreOutputsDisabled(void);
 
 }  // extern "C"
@@ -33,6 +34,7 @@
 
 class HALSimGui {
  public:
+  static void GlobalInit();
   static bool Initialize();
   static void Main(void*);
   static void Exit(void*);
@@ -128,6 +130,14 @@
   static void SetDefaultWindowSize(const char* name, float width, float height);
 
   /**
+   * Sets internal padding of window added with AddWindow().
+   * @param name window name (same as name passed to AddWindow())
+   * @param x horizontal padding
+   * @param y vertical padding
+   */
+  static void SetWindowPadding(const char* name, float x, float y);
+
+  /**
    * Returns true if outputs are disabled.
    *
    * @return true if outputs are disabled, false otherwise.

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

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

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

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

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

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

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

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

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

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

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

diff --git a/simulation/halsim_gui/src/main/native/include/IniSaverVector.h b/simulation/halsim_gui/src/main/native/include/IniSaverVector.h
new file mode 100644
index 0000000..0816933
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/include/IniSaverVector.h
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include <imgui.h>
+#include <imgui_internal.h>
+
+namespace halsimgui {
+
+template <typename Info>
+class IniSaverVector : public std::vector<Info> {
+ public:
+  explicit IniSaverVector(const char* typeName) : m_typeName(typeName) {}
+  void Initialize();
+
+ private:
+  static void* ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                        const char* name);
+  static void ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                       void* entry, const char* lineStr);
+  static void WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                       ImGuiTextBuffer* out_buf);
+
+  const char* m_typeName;
+};
+
+}  // namespace halsimgui
+
+#include "IniSaverVector.inl"

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

diff --git a/simulation/halsim_gui/src/main/native/include/IniSaverVector.inl b/simulation/halsim_gui/src/main/native/include/IniSaverVector.inl
new file mode 100644
index 0000000..b2979bc
--- /dev/null
+++ b/simulation/halsim_gui/src/main/native/include/IniSaverVector.inl
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace halsimgui {
+
+template <typename Info>
+void IniSaverVector<Info>::Initialize() {
+  // hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = m_typeName;
+  iniHandler.TypeHash = ImHashStr(m_typeName);
+  iniHandler.ReadOpenFn = ReadOpen;
+  iniHandler.ReadLineFn = ReadLine;
+  iniHandler.WriteAllFn = WriteAll;
+  iniHandler.UserData = this;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+}
+
+template <typename Info>
+void* IniSaverVector<Info>::ReadOpen(ImGuiContext* ctx,
+                                     ImGuiSettingsHandler* handler,
+                                     const char* name) {
+  auto self = static_cast<IniSaverVector*>(handler->UserData);
+  unsigned int num;
+  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
+  if (num >= self->size()) self->resize(num + 1);
+  return &(*self)[num];
+}
+
+template <typename Info>
+void IniSaverVector<Info>::ReadLine(ImGuiContext* ctx,
+                                    ImGuiSettingsHandler* handler, void* entry,
+                                    const char* lineStr) {
+  auto element = static_cast<Info*>(entry);
+  wpi::StringRef line{lineStr};
+  auto [name, value] = line.split('=');
+  name = name.trim();
+  value = value.trim();
+  element->ReadIni(name, value);
+}
+
+template <typename Info>
+void IniSaverVector<Info>::WriteAll(ImGuiContext* ctx,
+                                    ImGuiSettingsHandler* handler,
+                                    ImGuiTextBuffer* out_buf) {
+  auto self = static_cast<IniSaverVector*>(handler->UserData);
+  for (size_t i = 0; i < self->size(); ++i) {
+    out_buf->appendf("[%s][%d]\n", self->m_typeName, static_cast<int>(i));
+    (*self)[i].WriteIni(out_buf);
+    out_buf->append("\n");
+  }
+}
+
+}  // namespace halsimgui

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

diff --git a/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h b/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h
index 22f29b2..8c24cc9 100644
--- a/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h
+++ b/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -35,6 +35,8 @@
 
 namespace halsimgui {
 
+class GuiDataSource;
+
 class SimDeviceGui {
  public:
   static void Initialize();
@@ -70,6 +72,22 @@
                            int32_t numOptions = 0);
 
   /**
+   * Displays device value formatted the same way as SimDevice device values.
+   *
+   * @param name value name
+   * @param readonly prevent value from being modified by the user
+   * @param value value contents (modified in place)
+   * @param source data source (may be nullptr)
+   * @param options options array for enum values
+   * @param numOptions size of options array for enum values
+   * @return True if value was modified by the user
+   */
+  static bool DisplayValueSource(const char* name, bool readonly,
+                                 HAL_Value* value, const GuiDataSource* source,
+                                 const char** options = nullptr,
+                                 int32_t numOptions = 0);
+
+  /**
    * Wraps ImGui::CollapsingHeader() to provide consistency and open
    * persistence.  As with the ImGui function, returns true if the tree node
    * is expanded.  If returns true, call StopDevice() to finish the block.

simulation/halsim_lowfi/CMakeLists.txt

diff --git a/simulation/halsim_lowfi/CMakeLists.txt b/simulation/halsim_lowfi/CMakeLists.txt
deleted file mode 100644
index b4ce561..0000000
--- a/simulation/halsim_lowfi/CMakeLists.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-project(halsim_lowfi)
-
-include(CompileWarnings)
-
-file(GLOB halsim_lowfi_src src/main/native/cpp/*.cpp)
-
-add_library(halsim_lowfi MODULE ${halsim_lowfi_src})
-wpilib_target_warnings(halsim_lowfi)
-set_target_properties(halsim_lowfi PROPERTIES DEBUG_POSTFIX "d")
-target_link_libraries(halsim_lowfi PUBLIC hal ntcore)
-
-target_include_directories(halsim_lowfi PRIVATE src/main/native/include)
-
-set_property(TARGET halsim_lowfi PROPERTY FOLDER "libraries")
-
-install(TARGETS halsim_lowfi EXPORT halsim_lowfi DESTINATION "${main_lib_dest}")

simulation/halsim_lowfi/build.gradle

diff --git a/simulation/halsim_lowfi/build.gradle b/simulation/halsim_lowfi/build.gradle
deleted file mode 100644
index 549a43c..0000000
--- a/simulation/halsim_lowfi/build.gradle
+++ /dev/null
@@ -1,9 +0,0 @@
-description = "A simulation shared object that publishes basic robot I/O to NetworkTables."
-
-ext {
-    includeNtCore = true
-    includeWpiutil = true
-    pluginName = 'halsim_lowfi'
-}
-
-apply from: "${rootDir}/shared/plugins/setupBuild.gradle"

simulation/halsim_lowfi/src/dev/native/cpp/main.cpp

diff --git a/simulation/halsim_lowfi/src/dev/native/cpp/main.cpp b/simulation/halsim_lowfi/src/dev/native/cpp/main.cpp
deleted file mode 100644
index e324b44..0000000
--- a/simulation/halsim_lowfi/src/dev/native/cpp/main.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-int main() {}

simulation/halsim_lowfi/src/main/native/cpp/HALSimLowFi.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/HALSimLowFi.cpp b/simulation/halsim_lowfi/src/main/native/cpp/HALSimLowFi.cpp
deleted file mode 100644
index f82efb7..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/HALSimLowFi.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "HALSimLowFi.h"
-
-#include <wpi/Twine.h>
-
-void HALSimLowFi::Initialize() {
-  table = nt::NetworkTableInstance::GetDefault().GetTable("sim");
-}
-
-void HALSimNTProvider::Inject(std::shared_ptr<HALSimLowFi> parentArg,
-                              std::string tableNameArg) {
-  parent = parentArg;
-  tableName = std::move(tableNameArg);
-  table = parent->table->GetSubTable(tableName);
-
-  this->Initialize();
-}
-
-void NTProviderBaseCallback(const char* name, void* param,
-                            const struct HAL_Value* value) {
-  auto info =
-      static_cast<struct HALSimNTProvider::NTProviderCallbackInfo*>(param);
-  uint32_t chan = static_cast<uint32_t>(info->channel);
-  auto provider = info->provider;
-  auto table = info->table;
-  provider->OnCallback(chan, table);
-}
-
-void HALSimNTProvider::InitializeDefault(
-    int numChannels, HALCbRegisterIndexedFunc registerFunc) {
-  this->numChannels = numChannels;
-  cbInfos.reserve(numChannels);
-  for (int i = 0; i < numChannels; i++) {
-    struct NTProviderCallbackInfo info = {
-        this, table->GetSubTable(tableName + wpi::Twine(i)), i};
-    cbInfos.emplace_back(info);
-  }
-
-  for (auto& info : cbInfos) {
-    registerFunc(info.channel, NTProviderBaseCallback, &info, true);
-    OnInitializedChannel(info.channel, info.table);
-  }
-}
-
-void HALSimNTProvider::InitializeDefaultSingle(
-    HALCbRegisterSingleFunc registerFunc) {
-  struct NTProviderCallbackInfo info = {this, table, 0};
-  cbInfos.push_back(info);
-
-  for (auto& info : cbInfos) {
-    registerFunc(NTProviderBaseCallback, &info, true);
-  }
-}
-
-void HALSimNTProvider::OnInitializedChannel(
-    uint32_t channel, std::shared_ptr<nt::NetworkTable> table) {}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Analog.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Analog.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Analog.cpp
deleted file mode 100644
index 2d35c57..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Analog.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_Analog.h"
-
-#include <hal/Ports.h>
-#include <mockdata/AnalogInData.h>
-#include <mockdata/AnalogOutData.h>
-
-void HALSimNTProviderAnalogIn::Initialize() {
-  InitializeDefault(HAL_GetNumAnalogInputs(),
-                    HALSIM_RegisterAnalogInAllCallbacks);
-}
-
-void HALSimNTProviderAnalogIn::OnCallback(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("init?").SetBoolean(HALSIM_GetAnalogInInitialized(chan));
-  table->GetEntry("avg_bits").SetDouble(HALSIM_GetAnalogInAverageBits(chan));
-  table->GetEntry("oversample_bits")
-      .SetDouble(HALSIM_GetAnalogInOversampleBits(chan));
-  table->GetEntry("voltage").SetDouble(HALSIM_GetAnalogInVoltage(chan));
-
-  auto accum = table->GetSubTable("accum");
-  accum->GetEntry("init?").SetBoolean(
-      HALSIM_GetAnalogInAccumulatorInitialized(chan));
-  accum->GetEntry("value").SetDouble(HALSIM_GetAnalogInAccumulatorValue(chan));
-  accum->GetEntry("count").SetDouble(HALSIM_GetAnalogInAccumulatorCount(chan));
-  accum->GetEntry("center").SetDouble(
-      HALSIM_GetAnalogInAccumulatorCenter(chan));
-  accum->GetEntry("deadband")
-      .SetDouble(HALSIM_GetAnalogInAccumulatorDeadband(chan));
-}
-
-void HALSimNTProviderAnalogIn::OnInitializedChannel(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("voltage").AddListener(
-      [=](const nt::EntryNotification& ev) -> void {
-        HALSIM_SetAnalogInVoltage(chan, ev.value->GetDouble());
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-}
-
-void HALSimNTProviderAnalogOut::Initialize() {
-  InitializeDefault(HAL_GetNumAnalogOutputs(),
-                    HALSIM_RegisterAnalogOutAllCallbacks);
-}
-
-void HALSimNTProviderAnalogOut::OnCallback(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("init?").SetBoolean(HALSIM_GetAnalogOutInitialized(chan));
-  table->GetEntry("voltage").SetDouble(HALSIM_GetAnalogOutVoltage(chan));
-}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DIO.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DIO.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DIO.cpp
deleted file mode 100644
index f4ed9d6..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DIO.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_DIO.h"
-
-#include <hal/Ports.h>
-#include <mockdata/DIOData.h>
-
-void HALSimNTProviderDIO::Initialize() {
-  InitializeDefault(HAL_GetNumDigitalChannels(),
-                    HALSIM_RegisterDIOAllCallbacks);
-}
-
-void HALSimNTProviderDIO::OnCallback(uint32_t chan,
-                                     std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("init?").SetBoolean(HALSIM_GetDIOInitialized(chan));
-  table->GetEntry("value").SetBoolean(HALSIM_GetDIOValue(chan));
-  table->GetEntry("pulse_length").SetDouble(HALSIM_GetDIOPulseLength(chan));
-  table->GetEntry("input?").SetBoolean(HALSIM_GetDIOIsInput(chan));
-}
-
-void HALSimNTProviderDIO::OnInitializedChannel(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("value").AddListener(
-      [=](const nt::EntryNotification& ev) -> void {
-        if (HALSIM_GetDIOIsInput(chan)) {
-          HALSIM_SetDIOValue(chan, ev.value->GetBoolean());
-        }
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DriverStation.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DriverStation.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DriverStation.cpp
deleted file mode 100644
index e02d646..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_DriverStation.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_DriverStation.h"
-
-#include <hal/Ports.h>
-#include <mockdata/DriverStationData.h>
-
-void HALSimNTProviderDriverStation::Initialize() {
-  InitializeDefaultSingle(HALSIM_RegisterDriverStationAllCallbacks);
-}
-
-void HALSimNTProviderDriverStation::OnCallback(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  bool auton = HALSIM_GetDriverStationAutonomous(),
-       test = HALSIM_GetDriverStationTest(),
-       enabled = HALSIM_GetDriverStationEnabled();
-
-  bool teleop = (!auton && !test && enabled);
-
-  table->GetEntry("enabled?").SetBoolean(enabled);
-  table->GetEntry("autonomous?").SetBoolean(auton);
-  table->GetEntry("test?").SetBoolean(test);
-  table->GetEntry("teleop?").SetBoolean(teleop);
-  table->GetEntry("estop?").SetBoolean(HALSIM_GetDriverStationEStop());
-  table->GetEntry("fms?").SetBoolean(HALSIM_GetDriverStationFmsAttached());
-  table->GetEntry("ds?").SetBoolean(HALSIM_GetDriverStationDsAttached());
-  table->GetEntry("match_time").SetDouble(HALSIM_GetDriverStationMatchTime());
-
-  // TODO: Joysticks
-
-  auto alliance = table->GetSubTable("alliance");
-  auto allianceValue = HALSIM_GetDriverStationAllianceStationId();
-  alliance->GetEntry("color").SetString(
-      (allianceValue == HAL_AllianceStationID_kRed1 ||
-       allianceValue == HAL_AllianceStationID_kRed2 ||
-       allianceValue == HAL_AllianceStationID_kRed3)
-          ? "red"
-          : "blue");
-  int station = 0;
-
-  switch (allianceValue) {
-    case HAL_AllianceStationID_kRed1:
-    case HAL_AllianceStationID_kBlue1:
-      station = 1;
-      break;
-    case HAL_AllianceStationID_kRed2:
-    case HAL_AllianceStationID_kBlue2:
-      station = 2;
-      break;
-    case HAL_AllianceStationID_kRed3:
-    case HAL_AllianceStationID_kBlue3:
-      station = 3;
-      break;
-  }
-  alliance->GetEntry("station").SetDouble(station);
-}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Encoder.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Encoder.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Encoder.cpp
deleted file mode 100644
index ca012ab..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Encoder.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_Encoder.h"
-
-#include <hal/Ports.h>
-#include <mockdata/EncoderData.h>
-
-void HALSimNTProviderEncoder::Initialize() {
-  InitializeDefault(HAL_GetNumEncoders(), HALSIM_RegisterEncoderAllCallbacks);
-}
-
-void HALSimNTProviderEncoder::OnCallback(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("init?").SetBoolean(HALSIM_GetEncoderInitialized(chan));
-  table->GetEntry("count").SetDouble(HALSIM_GetEncoderCount(chan));
-  table->GetEntry("period").SetDouble(HALSIM_GetEncoderPeriod(chan));
-  table->GetEntry("reset?").SetBoolean(HALSIM_GetEncoderReset(chan));
-  table->GetEntry("max_period").SetDouble(HALSIM_GetEncoderMaxPeriod(chan));
-  table->GetEntry("direction").SetBoolean(HALSIM_GetEncoderDirection(chan));
-  table->GetEntry("reverse_direction?")
-      .SetBoolean(HALSIM_GetEncoderReverseDirection(chan));
-  table->GetEntry("samples_to_avg")
-      .SetDouble(HALSIM_GetEncoderSamplesToAverage(chan));
-}
-
-void HALSimNTProviderEncoder::OnInitializedChannel(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("count").AddListener(
-      [=](const nt::EntryNotification& ev) -> void {
-        HALSIM_SetEncoderCount(chan,
-                               static_cast<int32_t>(ev.value->GetDouble()));
-      },
-      NT_NotifyKind::NT_NOTIFY_UPDATE);
-
-  table->GetEntry("direction")
-      .AddListener(
-          [=](const nt::EntryNotification& ev) -> void {
-            HALSIM_SetEncoderDirection(chan, ev.value->GetBoolean());
-          },
-          NT_NotifyKind::NT_NOTIFY_UPDATE);
-}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_PWM.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_PWM.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_PWM.cpp
deleted file mode 100644
index 5786eee..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_PWM.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_PWM.h"
-
-#include <hal/Ports.h>
-#include <mockdata/PWMData.h>
-
-void HALSimNTProviderPWM::Initialize() {
-  InitializeDefault(HAL_GetNumPWMChannels(), HALSIM_RegisterPWMAllCallbacks);
-}
-
-void HALSimNTProviderPWM::OnCallback(uint32_t chan,
-                                     std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("init?").SetBoolean(HALSIM_GetPWMInitialized(chan));
-  table->GetEntry("speed").SetDouble(HALSIM_GetPWMSpeed(chan));
-  table->GetEntry("position").SetDouble(HALSIM_GetPWMPosition(chan));
-  table->GetEntry("raw").SetDouble(HALSIM_GetPWMRawValue(chan));
-  table->GetEntry("period_scale").SetDouble(HALSIM_GetPWMPeriodScale(chan));
-  table->GetEntry("zero_latch?").SetBoolean(HALSIM_GetPWMZeroLatch(chan));
-}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Relay.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Relay.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Relay.cpp
deleted file mode 100644
index 249a1c3..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_Relay.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_Relay.h"
-
-#include <hal/Ports.h>
-#include <mockdata/RelayData.h>
-
-void HALSimNTProviderRelay::Initialize() {
-  InitializeDefault(HAL_GetNumRelayHeaders(), HALSIM_RegisterRelayAllCallbacks);
-}
-
-void HALSimNTProviderRelay::OnCallback(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("init_fwd?")
-      .SetBoolean(HALSIM_GetRelayInitializedForward(chan));
-  table->GetEntry("init_rvs?")
-      .SetBoolean(HALSIM_GetRelayInitializedReverse(chan));
-  table->GetEntry("fwd?").SetBoolean(HALSIM_GetRelayForward(chan));
-  table->GetEntry("rvs?").SetBoolean(HALSIM_GetRelayReverse(chan));
-}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_RoboRIO.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_RoboRIO.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_RoboRIO.cpp
deleted file mode 100644
index c3eaa25..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_RoboRIO.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_RoboRIO.h"
-
-#include <hal/Ports.h>
-#include <mockdata/RoboRioData.h>
-
-void HALSimNTProviderRoboRIO::Initialize() {
-  InitializeDefault(1, HALSIM_RegisterRoboRioAllCallbacks);
-}
-
-void HALSimNTProviderRoboRIO::OnCallback(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("fpga_button?").SetBoolean(HALSIM_GetRoboRioFPGAButton(chan));
-
-  table->GetEntry("vin_voltage").SetDouble(HALSIM_GetRoboRioVInVoltage(chan));
-  table->GetEntry("vin_current").SetDouble(HALSIM_GetRoboRioVInCurrent(chan));
-
-  auto t6v = table->GetSubTable("6V");
-  t6v->GetEntry("voltage").SetDouble(HALSIM_GetRoboRioUserVoltage6V(chan));
-  t6v->GetEntry("current").SetDouble(HALSIM_GetRoboRioUserCurrent6V(chan));
-  t6v->GetEntry("active?").SetBoolean(HALSIM_GetRoboRioUserActive6V(chan));
-  t6v->GetEntry("faults").SetDouble(HALSIM_GetRoboRioUserFaults6V(chan));
-
-  auto t5v = table->GetSubTable("5V");
-  t5v->GetEntry("voltage").SetDouble(HALSIM_GetRoboRioUserVoltage5V(chan));
-  t5v->GetEntry("current").SetDouble(HALSIM_GetRoboRioUserCurrent5V(chan));
-  t5v->GetEntry("active?").SetBoolean(HALSIM_GetRoboRioUserActive5V(chan));
-  t5v->GetEntry("faults").SetDouble(HALSIM_GetRoboRioUserFaults5V(chan));
-
-  auto t3v3 = table->GetSubTable("3V3");
-  t3v3->GetEntry("voltage").SetDouble(HALSIM_GetRoboRioUserVoltage3V3(chan));
-  t3v3->GetEntry("current").SetDouble(HALSIM_GetRoboRioUserCurrent3V3(chan));
-  t3v3->GetEntry("active?").SetBoolean(HALSIM_GetRoboRioUserActive3V3(chan));
-  t3v3->GetEntry("faults").SetDouble(HALSIM_GetRoboRioUserFaults3V3(chan));
-}
-
-void HALSimNTProviderRoboRIO::OnInitializedChannel(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("fpga_button?")
-      .AddListener(
-          [=](const nt::EntryNotification& ev) -> void {
-            HALSIM_SetRoboRioFPGAButton(chan, ev.value->GetBoolean());
-          },
-          NT_NotifyKind::NT_NOTIFY_UPDATE);
-}

simulation/halsim_lowfi/src/main/native/cpp/NTProvider_dPWM.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_dPWM.cpp b/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_dPWM.cpp
deleted file mode 100644
index e2bb74a..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/NTProvider_dPWM.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "NTProvider_dPWM.h"
-
-#include <hal/Ports.h>
-#include <mockdata/DigitalPWMData.h>
-
-void HALSimNTProviderDigitalPWM::Initialize() {
-  InitializeDefault(HAL_GetNumDigitalPWMOutputs(),
-                    HALSIM_RegisterDigitalPWMAllCallbacks);
-}
-
-void HALSimNTProviderDigitalPWM::OnCallback(
-    uint32_t chan, std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("init?").SetBoolean(HALSIM_GetDigitalPWMInitialized(chan));
-  table->GetEntry("dio_pin").SetDouble(HALSIM_GetDigitalPWMPin(chan));
-  table->GetEntry("duty_cycle").SetDouble(HALSIM_GetDigitalPWMDutyCycle(chan));
-}

simulation/halsim_lowfi/src/main/native/cpp/main.cpp

diff --git a/simulation/halsim_lowfi/src/main/native/cpp/main.cpp b/simulation/halsim_lowfi/src/main/native/cpp/main.cpp
deleted file mode 100644
index b94a634..0000000
--- a/simulation/halsim_lowfi/src/main/native/cpp/main.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <iostream>
-
-#include <HALSimLowFi.h>
-#include <NTProvider_Analog.h>
-#include <NTProvider_DIO.h>
-#include <NTProvider_DriverStation.h>
-#include <NTProvider_Encoder.h>
-#include <NTProvider_PWM.h>
-#include <NTProvider_Relay.h>
-#include <NTProvider_RoboRIO.h>
-#include <NTProvider_dPWM.h>
-
-static HALSimLowFi halsim_lowfi;
-
-static HALSimNTProviderPWM pwm_provider;
-static HALSimNTProviderDigitalPWM dpwm_provider;
-static HALSimNTProviderDIO dio_provider;
-static HALSimNTProviderAnalogIn ai_provider;
-static HALSimNTProviderAnalogOut ao_provider;
-static HALSimNTProviderDriverStation ds_provider;
-static HALSimNTProviderEncoder encoder_provider;
-static HALSimNTProviderRelay relay_provider;
-static HALSimNTProviderRoboRIO roborio_provider;
-
-extern "C" {
-#if defined(WIN32) || defined(_WIN32)
-__declspec(dllexport)
-#endif
-    int HALSIM_InitExtension(void) {
-  std::cout << "NetworkTables LowFi Simulator Initializing." << std::endl;
-  halsim_lowfi.Initialize();
-  halsim_lowfi.table->GetInstance().StartServer("networktables.ini");
-  halsim_lowfi.table->GetInstance().SetUpdateRate(0.05);
-  auto lowfi = std::make_shared<HALSimLowFi>(halsim_lowfi);
-
-  pwm_provider.Inject(lowfi, "PWM");
-  dpwm_provider.Inject(lowfi, "dPWM");
-  dio_provider.Inject(lowfi, "DIO");
-  ai_provider.Inject(lowfi, "AI");
-  ao_provider.Inject(lowfi, "AO");
-  ds_provider.Inject(lowfi, "DriverStation");
-  encoder_provider.Inject(lowfi, "Encoder");
-  relay_provider.Inject(lowfi, "Relay");
-  roborio_provider.Inject(lowfi, "RoboRIO");
-
-  std::cout << "NetworkTables LowFi Simulator Initialized!" << std::endl;
-  return 0;
-}
-}  // extern "C"

simulation/halsim_lowfi/src/main/native/include/HALSimLowFi.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/HALSimLowFi.h b/simulation/halsim_lowfi/src/main/native/include/HALSimLowFi.h
deleted file mode 100644
index 28faed0..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/HALSimLowFi.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <cinttypes>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <mockdata/NotifyListener.h>
-#include <networktables/NetworkTableInstance.h>
-
-class HALSimLowFi {
- public:
-  std::shared_ptr<nt::NetworkTable> table;
-  void Initialize();
-};
-
-typedef void (*HALCbRegisterIndexedFunc)(int32_t index,
-                                         HAL_NotifyCallback callback,
-                                         void* param, HAL_Bool initialNotify);
-typedef void (*HALCbRegisterSingleFunc)(HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-
-void NTProviderBaseCallback(const char* name, void* param,
-                            const struct HAL_Value* value);
-
-class HALSimNTProvider {
- public:
-  struct NTProviderCallbackInfo {
-    HALSimNTProvider* provider;
-    std::shared_ptr<nt::NetworkTable> table;
-    int channel;
-  };
-
-  void Inject(std::shared_ptr<HALSimLowFi> parent, std::string table);
-  // Initialize is called by inject.
-  virtual void Initialize() = 0;
-  virtual void InitializeDefault(int numChannels,
-                                 HALCbRegisterIndexedFunc registerFunc);
-  virtual void InitializeDefaultSingle(HALCbRegisterSingleFunc registerFunc);
-  virtual void OnCallback(uint32_t channel,
-                          std::shared_ptr<nt::NetworkTable> table) = 0;
-  virtual void OnInitializedChannel(uint32_t channel,
-                                    std::shared_ptr<nt::NetworkTable> table);
-
-  int numChannels;
-  std::string tableName;
-
-  std::shared_ptr<HALSimLowFi> parent;
-  std::shared_ptr<nt::NetworkTable> table;
-  std::vector<NTProviderCallbackInfo> cbInfos;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_Analog.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_Analog.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_Analog.h
deleted file mode 100644
index 8672b46..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_Analog.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderAnalogIn : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-  void OnInitializedChannel(uint32_t channel,
-                            std::shared_ptr<nt::NetworkTable> table) override;
-};
-
-class HALSimNTProviderAnalogOut : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_DIO.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_DIO.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_DIO.h
deleted file mode 100644
index b584b48..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_DIO.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderDIO : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-  void OnInitializedChannel(uint32_t channel,
-                            std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_DriverStation.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_DriverStation.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_DriverStation.h
deleted file mode 100644
index 5041b02..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_DriverStation.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderDriverStation : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_Encoder.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_Encoder.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_Encoder.h
deleted file mode 100644
index f3ef70f..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_Encoder.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderEncoder : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-  void OnInitializedChannel(uint32_t channel,
-                            std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_PWM.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_PWM.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_PWM.h
deleted file mode 100644
index 47c416e..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_PWM.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderPWM : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_Relay.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_Relay.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_Relay.h
deleted file mode 100644
index cc8439a..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_Relay.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderRelay : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_RoboRIO.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_RoboRIO.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_RoboRIO.h
deleted file mode 100644
index 055b6e6..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_RoboRIO.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderRoboRIO : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-  void OnInitializedChannel(uint32_t channel,
-                            std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_lowfi/src/main/native/include/NTProvider_dPWM.h

diff --git a/simulation/halsim_lowfi/src/main/native/include/NTProvider_dPWM.h b/simulation/halsim_lowfi/src/main/native/include/NTProvider_dPWM.h
deleted file mode 100644
index 0af607f..0000000
--- a/simulation/halsim_lowfi/src/main/native/include/NTProvider_dPWM.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <HALSimLowFi.h>
-
-class HALSimNTProviderDigitalPWM : public HALSimNTProvider {
- public:
-  void Initialize() override;
-  void OnCallback(uint32_t channel,
-                  std::shared_ptr<nt::NetworkTable> table) override;
-};

simulation/halsim_print/CMakeLists.txt

diff --git a/simulation/halsim_print/CMakeLists.txt b/simulation/halsim_print/CMakeLists.txt
deleted file mode 100644
index f168cab..0000000
--- a/simulation/halsim_print/CMakeLists.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-project(halsim_print)
-
-include(CompileWarnings)
-
-file(GLOB halsim_print_src src/main/native/cpp/*.cpp)
-
-add_library(halsim_print MODULE ${halsim_print_src})
-wpilib_target_warnings(halsim_print)
-set_target_properties(halsim_print PROPERTIES DEBUG_POSTFIX "d")
-target_link_libraries(halsim_print PUBLIC hal)
-
-target_include_directories(halsim_print PRIVATE src/main/native/include)
-
-set_property(TARGET halsim_print PROPERTY FOLDER "libraries")
-
-install(TARGETS halsim_print EXPORT halsim_print DESTINATION "${main_lib_dest}")

simulation/halsim_print/build.gradle

diff --git a/simulation/halsim_print/build.gradle b/simulation/halsim_print/build.gradle
deleted file mode 100644
index dd0a347..0000000
--- a/simulation/halsim_print/build.gradle
+++ /dev/null
@@ -1,7 +0,0 @@
-description = "A simulation shared object that simply prints robot behaviors"
-
-ext {
-    pluginName = 'halsim_print'
-}
-
-apply from: "${rootDir}/shared/plugins/setupBuild.gradle"

simulation/halsim_print/src/dev/native/cpp/main.cpp

diff --git a/simulation/halsim_print/src/dev/native/cpp/main.cpp b/simulation/halsim_print/src/dev/native/cpp/main.cpp
deleted file mode 100644
index eb9eef1..0000000
--- a/simulation/halsim_print/src/dev/native/cpp/main.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <chrono>
-#include <thread>
-
-#include <hal/HALBase.h>
-
-int main() {
-  HAL_Initialize(500, 0);
-  std::this_thread::sleep_for(std::chrono::seconds(2));
-}

simulation/halsim_print/src/main/native/cpp/PrintPWM.cpp

diff --git a/simulation/halsim_print/src/main/native/cpp/PrintPWM.cpp b/simulation/halsim_print/src/main/native/cpp/PrintPWM.cpp
deleted file mode 100644
index d271ef1..0000000
--- a/simulation/halsim_print/src/main/native/cpp/PrintPWM.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "PrintPWM.h"
-
-#include <iostream>
-
-#include <hal/Value.h>
-#include <mockdata/NotifyListener.h>
-#include <mockdata/PWMData.h>
-
-static void PWMCallback(const char* name, void* param,
-                        const struct HAL_Value* value) {
-  auto pwm = static_cast<PrintPWM*>(param);
-  pwm->Publish(value->data.v_double);
-}
-
-PrintPWM::PrintPWM(int port) {
-  m_port = port;
-  HALSIM_RegisterPWMSpeedCallback(port, PWMCallback, this, false);
-}
-
-void PrintPWM::Publish(double value) {
-  std::cout << "PWM " << m_port << ": " << value << std::endl;
-}

simulation/halsim_print/src/main/native/cpp/main.cpp

diff --git a/simulation/halsim_print/src/main/native/cpp/main.cpp b/simulation/halsim_print/src/main/native/cpp/main.cpp
deleted file mode 100644
index 55327ee..0000000
--- a/simulation/halsim_print/src/main/native/cpp/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <iostream>
-
-#include <hal/Ports.h>
-
-#include "HALSimPrint.h"
-#include "PrintPWM.h"
-
-// Currently, robots never terminate, so we keep a single static object and it
-// is never properly released or cleaned up.
-static HALSimPrint halsim;
-
-extern "C" {
-#if defined(WIN32) || defined(_WIN32)
-__declspec(dllexport)
-#endif
-    int HALSIM_InitExtension(void) {
-  std::cout << "Print Simulator Initializing." << std::endl;
-
-  int pwmCount = HAL_GetNumPWMChannels();
-  halsim.m_pwms.reserve(pwmCount);
-  for (int i = 0; i < pwmCount; i++) halsim.m_pwms.emplace_back(i);
-
-  return 0;
-}
-}  // extern "C"

simulation/halsim_print/src/main/native/include/HALSimPrint.h

diff --git a/simulation/halsim_print/src/main/native/include/HALSimPrint.h b/simulation/halsim_print/src/main/native/include/HALSimPrint.h
deleted file mode 100644
index 69dafec..0000000
--- a/simulation/halsim_print/src/main/native/include/HALSimPrint.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <vector>
-
-class PrintPWM;
-
-class HALSimPrint {
- public:
-  std::vector<PrintPWM> m_pwms;
-};

simulation/halsim_print/src/main/native/include/PrintPWM.h

diff --git a/simulation/halsim_print/src/main/native/include/PrintPWM.h b/simulation/halsim_print/src/main/native/include/PrintPWM.h
deleted file mode 100644
index 98803ff..0000000
--- a/simulation/halsim_print/src/main/native/include/PrintPWM.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "HALSimPrint.h"
-
-class PrintPWM {
- public:
-  explicit PrintPWM(int port);
-  void Publish(double value);
-
- private:
-  int m_port;
-};

simulation/halsim_ws_client/CMakeLists.txt

diff --git a/simulation/halsim_ws_client/CMakeLists.txt b/simulation/halsim_ws_client/CMakeLists.txt
new file mode 100644
index 0000000..d869f28
--- /dev/null
+++ b/simulation/halsim_ws_client/CMakeLists.txt
@@ -0,0 +1,16 @@
+project(halsim_ws_client)
+
+include(CompileWarnings)
+
+file(GLOB halsim_ws_client_src src/main/native/cpp/*.cpp)
+
+add_library(halsim_ws_client MODULE ${halsim_ws_client_src})
+wpilib_target_warnings(halsim_ws_client)
+set_target_properties(halsim_ws_client PROPERTIES DEBUG_POSTFIX "d")
+target_link_libraries(halsim_ws_client PUBLIC hal halsim_ws_core)
+
+target_include_directories(halsim_ws_client PRIVATE src/main/native/include)
+
+set_property(TARGET halsim_ws_client PROPERTY FOLDER "libraries")
+
+install(TARGETS halsim_ws_client EXPORT halsim_ws_client DESTINATION "${main_lib_dest}")

simulation/halsim_ws_client/build.gradle

diff --git a/simulation/halsim_ws_client/build.gradle b/simulation/halsim_ws_client/build.gradle
new file mode 100644
index 0000000..c0137a4
--- /dev/null
+++ b/simulation/halsim_ws_client/build.gradle
@@ -0,0 +1,35 @@
+if (!project.hasProperty('onlylinuxathena')) {
+
+    description = "WebSocket Client Extension"
+
+    ext {
+        includeWpiutil = true
+        pluginName = 'halsim_ws_client'
+    }
+
+    apply plugin: 'google-test-test-suite'
+
+
+    ext {
+        staticGtestConfigs = [:]
+    }
+
+    staticGtestConfigs["${pluginName}Test"] = []
+    apply from: "${rootDir}/shared/googletest.gradle"
+
+    apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
+
+    model {
+        binaries {
+            all {
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    it.buildable = false
+                    return
+                }
+
+                lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static"
+
+            }
+        }
+    }
+}

simulation/halsim_ws_client/src/dev/native/cpp/main.cpp

diff --git a/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp b/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..1efcefc
--- /dev/null
+++ b/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+#include <thread>
+
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
+#include <hal/Main.h>
+
+extern "C" int HALSIM_InitExtension(void);
+
+int main() {
+  HAL_Initialize(500, 0);
+  HALSIM_InitExtension();
+
+  HAL_RunMain();
+
+  int cycleCount = 0;
+  while (cycleCount < 100) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    cycleCount++;
+    std::cout << "Count: " << cycleCount << std::endl;
+  }
+
+  std::cout << "DONE" << std::endl;
+  HAL_ExitMain();
+}

simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp

diff --git a/simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp
new file mode 100644
index 0000000..0b40a61
--- /dev/null
+++ b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp
@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimWS.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/uv/util.h>
+
+#include "HALSimWSClientConnection.h"
+
+static constexpr int kTcpConnectAttemptTimeout = 1000;
+
+namespace uv = wpi::uv;
+
+using namespace wpilibws;
+
+HALSimWS::HALSimWS(wpi::uv::Loop& loop, ProviderContainer& providers,
+                   HALSimWSProviderSimDevices& simDevicesProvider)
+    : m_loop(loop),
+      m_providers(providers),
+      m_simDevicesProvider(simDevicesProvider) {
+  m_loop.error.connect([](uv::Error err) {
+    wpi::errs() << "HALSim WS Client libuv Error: " << err.str() << "\n";
+  });
+
+  m_tcp_client = uv::Tcp::Create(m_loop);
+  m_exec = UvExecFunc::Create(m_loop);
+  if (m_exec) {
+    m_exec->wakeup.connect([](auto func) { func(); });
+  }
+  m_connect_timer = uv::Timer::Create(m_loop);
+}
+
+bool HALSimWS::Initialize() {
+  if (!m_tcp_client || !m_exec || !m_connect_timer) {
+    return false;
+  }
+
+  const char* host = std::getenv("HALSIMWS_HOST");
+  if (host != NULL) {
+    m_host = host;
+  } else {
+    m_host = "localhost";
+  }
+
+  const char* port = std::getenv("HALSIMWS_PORT");
+  if (port != NULL) {
+    try {
+      m_port = std::stoi(port);
+    } catch (const std::invalid_argument& err) {
+      wpi::errs() << "Error decoding HALSIMWS_PORT (" << err.what() << ")\n";
+      return false;
+    }
+  } else {
+    m_port = 8080;
+  }
+
+  const char* uri = std::getenv("HALSIMWS_URI");
+  if (uri != NULL) {
+    m_uri = uri;
+  } else {
+    m_uri = "/wpilibws";
+  }
+
+  return true;
+}
+
+void HALSimWS::Start() {
+  m_tcp_client->SetNoDelay(true);
+
+  // Hook up TCP client events
+  m_tcp_client->error.connect(
+      [this, socket = m_tcp_client.get()](wpi::uv::Error err) {
+        if (m_tcp_connected) {
+          m_tcp_connected = false;
+          m_connect_attempts = 0;
+          return;
+        }
+
+        // If we weren't previously connected, attempt a reconnection
+        m_connect_timer->Start(uv::Timer::Time(kTcpConnectAttemptTimeout));
+      });
+
+  m_tcp_client->closed.connect(
+      []() { wpi::outs() << "TCP connection closed\n"; });
+
+  // Set up the connection timer
+  wpi::outs() << "HALSimWS Initialized\n";
+  wpi::outs() << "Will attempt to connect to ws://" << m_host << ":" << m_port
+              << m_uri << "\n";
+
+  // Set up the timer to attempt connection
+  m_connect_timer->timeout.connect([this] { AttemptConnect(); });
+
+  // Run the initial connect immediately
+  m_connect_timer->Start(uv::Timer::Time(0));
+  m_connect_timer->Unreference();
+}
+
+void HALSimWS::AttemptConnect() {
+  m_connect_attempts++;
+
+  wpi::outs() << "Connection Attempt " << m_connect_attempts << "\n";
+
+  struct sockaddr_in dest;
+  uv::NameToAddr(m_host, m_port, &dest);
+
+  m_tcp_client->Connect(dest, [this, socket = m_tcp_client.get()]() {
+    m_tcp_connected = true;
+    auto wsConn = std::make_shared<HALSimWSClientConnection>(shared_from_this(),
+                                                             m_tcp_client);
+
+    wsConn->Initialize();
+  });
+}
+
+bool HALSimWS::RegisterWebsocket(
+    std::shared_ptr<HALSimBaseWebSocketConnection> hws) {
+  if (m_hws.lock()) {
+    return false;
+  }
+
+  m_hws = hws;
+
+  m_simDevicesProvider.OnNetworkConnected(hws);
+
+  m_providers.ForEach([hws](std::shared_ptr<HALSimWSBaseProvider> provider) {
+    provider->OnNetworkConnected(hws);
+  });
+
+  return true;
+}
+
+void HALSimWS::CloseWebsocket(
+    std::shared_ptr<HALSimBaseWebSocketConnection> hws) {
+  // Inform the providers that they need to cancel callbacks
+  m_simDevicesProvider.OnNetworkDisconnected();
+
+  m_providers.ForEach([](std::shared_ptr<HALSimWSBaseProvider> provider) {
+    provider->OnNetworkDisconnected();
+  });
+
+  if (hws == m_hws.lock()) {
+    m_hws.reset();
+  }
+}
+
+void HALSimWS::OnNetValueChanged(const wpi::json& msg) {
+  // Look for "type" and "device" fields so that we can
+  // generate the key
+
+  try {
+    auto& type = msg.at("type").get_ref<const std::string&>();
+    auto& device = msg.at("device").get_ref<const std::string&>();
+
+    wpi::SmallString<64> key;
+    key.append(type);
+    if (!device.empty()) {
+      key.append("/");
+      key.append(device);
+    }
+
+    auto provider = m_providers.Get(key.str());
+    if (provider) {
+      provider->OnNetValueChanged(msg.at("data"));
+    }
+  } catch (wpi::json::exception& e) {
+    wpi::errs() << "Error with incoming message: " << e.what() << "\n";
+  }
+}

simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp

diff --git a/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp
new file mode 100644
index 0000000..f019e27
--- /dev/null
+++ b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimWSClient.h"
+
+#include <WSProviderContainer.h>
+#include <WSProvider_Analog.h>
+#include <WSProvider_DIO.h>
+#include <WSProvider_DriverStation.h>
+#include <WSProvider_Encoder.h>
+#include <WSProvider_Joystick.h>
+#include <WSProvider_PWM.h>
+#include <WSProvider_Relay.h>
+#include <WSProvider_RoboRIO.h>
+#include <WSProvider_SimDevice.h>
+#include <WSProvider_dPWM.h>
+#include <wpi/EventLoopRunner.h>
+
+using namespace wpilibws;
+
+bool HALSimWSClient::Initialize() {
+  bool result = true;
+  runner.ExecSync([&](wpi::uv::Loop& loop) {
+    simws = std::make_shared<HALSimWS>(loop, providers, simDevices);
+
+    if (!simws->Initialize()) {
+      result = false;
+      return;
+    }
+
+    WSRegisterFunc registerFunc = [&](auto key, auto provider) {
+      providers.Add(key, provider);
+    };
+
+    HALSimWSProviderAnalogIn::Initialize(registerFunc);
+    HALSimWSProviderAnalogOut::Initialize(registerFunc);
+    HALSimWSProviderDIO::Initialize(registerFunc);
+    HALSimWSProviderDigitalPWM::Initialize(registerFunc);
+    HALSimWSProviderDriverStation::Initialize(registerFunc);
+    HALSimWSProviderEncoder::Initialize(registerFunc);
+    HALSimWSProviderJoystick::Initialize(registerFunc);
+    HALSimWSProviderPWM::Initialize(registerFunc);
+    HALSimWSProviderRelay::Initialize(registerFunc);
+    HALSimWSProviderRoboRIO::Initialize(registerFunc);
+
+    simDevices.Initialize(loop);
+
+    simws->Start();
+  });
+
+  return result;
+}

simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp

diff --git a/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp
new file mode 100644
index 0000000..5cfec5c
--- /dev/null
+++ b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimWSClientConnection.h"
+
+#include <wpi/raw_ostream.h>
+#include <wpi/raw_uv_ostream.h>
+
+#include "HALSimWS.h"
+
+namespace uv = wpi::uv;
+
+using namespace wpilibws;
+
+void HALSimWSClientConnection::Initialize() {
+  // Get a shared pointer to ourselves
+  auto self = this->shared_from_this();
+
+  auto ws =
+      wpi::WebSocket::CreateClient(*m_stream, m_client->GetTargetUri(),
+                                   wpi::Twine{m_client->GetTargetHost()} + ":" +
+                                       wpi::Twine{m_client->GetTargetPort()});
+
+  ws->SetData(self);
+
+  m_websocket = ws.get();
+
+  // Hook up events
+  m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) {
+    conn.disconnect();
+
+    if (!m_client->RegisterWebsocket(shared_from_this())) {
+      wpi::errs() << "Unable to register websocket\n";
+      return;
+    }
+
+    m_ws_connected = true;
+    wpi::outs() << "HALSimWS: WebSocket Connected\n";
+  });
+
+  m_websocket->text.connect([this](wpi::StringRef msg, bool) {
+    if (!m_ws_connected) {
+      return;
+    }
+
+    wpi::json j;
+    try {
+      j = wpi::json::parse(msg);
+    } catch (const wpi::json::parse_error& e) {
+      std::string err("JSON parse failed: ");
+      err += e.what();
+      wpi::errs() << err << "\n";
+      m_websocket->Fail(1003, err);
+      return;
+    }
+
+    m_client->OnNetValueChanged(j);
+  });
+
+  m_websocket->closed.connect([this](uint16_t, wpi::StringRef) {
+    if (m_ws_connected) {
+      wpi::outs() << "HALSimWS: Websocket Disconnected\n";
+      m_ws_connected = false;
+
+      m_client->CloseWebsocket(shared_from_this());
+    }
+  });
+}
+
+void HALSimWSClientConnection::OnSimValueChanged(const wpi::json& msg) {
+  if (msg.empty()) {
+    return;
+  }
+  wpi::SmallVector<uv::Buffer, 4> sendBufs;
+  wpi::raw_uv_ostream os{sendBufs, [this]() -> uv::Buffer {
+                           std::lock_guard lock(m_buffers_mutex);
+                           return m_buffers.Allocate();
+                         }};
+
+  os << msg;
+
+  // Call the websocket send function on the uv loop
+  m_client->GetExec().Send([self = shared_from_this(), sendBufs] {
+    self->m_websocket->SendText(sendBufs,
+                                [self](auto bufs, wpi::uv::Error err) {
+                                  {
+                                    std::lock_guard lock(self->m_buffers_mutex);
+                                    self->m_buffers.Release(bufs);
+                                  }
+
+                                  if (err) {
+                                    wpi::errs() << err.str() << "\n";
+                                    wpi::errs().flush();
+                                  }
+                                });
+  });
+}

simulation/halsim_ws_client/src/main/native/cpp/main.cpp

diff --git a/simulation/halsim_ws_client/src/main/native/cpp/main.cpp b/simulation/halsim_ws_client/src/main/native/cpp/main.cpp
new file mode 100644
index 0000000..1112e61
--- /dev/null
+++ b/simulation/halsim_ws_client/src/main/native/cpp/main.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+
+#include <hal/Extensions.h>
+#include <wpi/raw_ostream.h>
+
+#include "HALSimWSClient.h"
+
+using namespace wpilibws;
+
+static std::unique_ptr<HALSimWSClient> gClient;
+
+extern "C" {
+#if defined(WIN32) || defined(_WIN32)
+__declspec(dllexport)
+#endif
+
+    int HALSIM_InitExtension(void) {
+  wpi::outs() << "HALSim WS Client Extension Initializing\n";
+
+  HAL_OnShutdown(nullptr, [](void*) { gClient.reset(); });
+
+  gClient = std::make_unique<HALSimWSClient>();
+  if (!gClient->Initialize()) {
+    return -1;
+  }
+
+  wpi::outs() << "HALSim WS Client Extension Initialized\n";
+  return 0;
+}
+
+}  // extern "C"

simulation/halsim_ws_client/src/main/native/include/HALSimWS.h

diff --git a/simulation/halsim_ws_client/src/main/native/include/HALSimWS.h b/simulation/halsim_ws_client/src/main/native/include/HALSimWS.h
new file mode 100644
index 0000000..e126ac1
--- /dev/null
+++ b/simulation/halsim_ws_client/src/main/native/include/HALSimWS.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpi/uv/Async.h>
+#include <wpi/uv/Loop.h>
+#include <wpi/uv/Tcp.h>
+#include <wpi/uv/Timer.h>
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace wpilibws {
+
+class HALSimWSClientConnection;
+
+class HALSimWS : public std::enable_shared_from_this<HALSimWS> {
+ public:
+  using LoopFunc = std::function<void(void)>;
+  using UvExecFunc = wpi::uv::Async<LoopFunc>;
+
+  HALSimWS(wpi::uv::Loop& loop, ProviderContainer& providers,
+           HALSimWSProviderSimDevices& simDevicesProvider);
+  HALSimWS(const HALSimWS&) = delete;
+  HALSimWS& operator=(const HALSimWS&) = delete;
+
+  bool Initialize();
+  void Start();
+
+  bool RegisterWebsocket(std::shared_ptr<HALSimBaseWebSocketConnection> hws);
+  void CloseWebsocket(std::shared_ptr<HALSimBaseWebSocketConnection> hws);
+
+  void OnNetValueChanged(const wpi::json& msg);
+
+  wpi::StringRef GetTargetHost() const { return m_host; }
+  wpi::StringRef GetTargetUri() const { return m_uri; }
+  int GetTargetPort() const { return m_port; }
+  wpi::uv::Loop& GetLoop() { return m_loop; }
+
+  UvExecFunc& GetExec() { return *m_exec; }
+
+ private:
+  void AttemptConnect();
+
+  bool m_tcp_connected = false;
+  std::shared_ptr<wpi::uv::Timer> m_connect_timer;
+  int m_connect_attempts = 0;
+
+  std::weak_ptr<HALSimBaseWebSocketConnection> m_hws;
+
+  wpi::uv::Loop& m_loop;
+  std::shared_ptr<wpi::uv::Tcp> m_tcp_client;
+  std::shared_ptr<UvExecFunc> m_exec;
+
+  ProviderContainer& m_providers;
+  HALSimWSProviderSimDevices& m_simDevicesProvider;
+
+  std::string m_host;
+  std::string m_uri;
+  int m_port;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h

diff --git a/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h
new file mode 100644
index 0000000..5812140
--- /dev/null
+++ b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpi/EventLoopRunner.h>
+
+#include "HALSimWS.h"
+
+namespace wpilibws {
+
+class HALSimWSClient {
+ public:
+  HALSimWSClient() = default;
+  HALSimWSClient(const HALSimWSClient&) = delete;
+  HALSimWSClient& operator=(const HALSimWSClient&) = delete;
+
+  bool Initialize();
+
+  ProviderContainer providers;
+  HALSimWSProviderSimDevices simDevices{providers};
+  wpi::EventLoopRunner runner;
+  std::shared_ptr<HALSimWS> simws;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h

diff --git a/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h
new file mode 100644
index 0000000..1bd23e9
--- /dev/null
+++ b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+
+#include <HALSimBaseWebSocketConnection.h>
+#include <wpi/WebSocket.h>
+#include <wpi/mutex.h>
+#include <wpi/uv/Buffer.h>
+#include <wpi/uv/Stream.h>
+
+#include "HALSimWS.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace wpilibws {
+
+class HALSimWS;
+
+class HALSimWSClientConnection
+    : public HALSimBaseWebSocketConnection,
+      public std::enable_shared_from_this<HALSimWSClientConnection> {
+ public:
+  explicit HALSimWSClientConnection(std::shared_ptr<HALSimWS> client,
+                                    std::shared_ptr<wpi::uv::Stream> stream)
+      : m_client(std::move(client)),
+        m_stream(std::move(stream)),
+        m_buffers(128) {}
+
+ public:
+  void OnSimValueChanged(const wpi::json& msg) override;
+  void Initialize();
+
+ private:
+  std::shared_ptr<HALSimWS> m_client;
+  std::shared_ptr<wpi::uv::Stream> m_stream;
+
+  bool m_ws_connected = false;
+  wpi::WebSocket* m_websocket = nullptr;
+
+  wpi::uv::SimpleBufferPool<4> m_buffers;
+  std::mutex m_buffers_mutex;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/CMakeLists.txt

diff --git a/simulation/halsim_ws_core/CMakeLists.txt b/simulation/halsim_ws_core/CMakeLists.txt
new file mode 100644
index 0000000..56b1766
--- /dev/null
+++ b/simulation/halsim_ws_core/CMakeLists.txt
@@ -0,0 +1,16 @@
+project(halsim_ws_core)
+
+include(CompileWarnings)
+
+file(GLOB halsim_ws_core_src src/main/native/cpp/*.cpp)
+
+add_library(halsim_ws_core STATIC ${halsim_ws_core_src})
+wpilib_target_warnings(halsim_ws_core)
+set_target_properties(halsim_ws_core PROPERTIES DEBUG_POSTFIX "d" POSITION_INDEPENDENT_CODE ON)
+target_link_libraries(halsim_ws_core PUBLIC hal)
+
+target_include_directories(halsim_ws_core PUBLIC src/main/native/include)
+
+set_property(TARGET halsim_ws_core PROPERTY FOLDER "libraries")
+
+install(TARGETS halsim_ws_core EXPORT halsim_ws_core DESTINATION "${main_lib_dest}")

simulation/halsim_ws_core/build.gradle

diff --git a/simulation/halsim_ws_core/build.gradle b/simulation/halsim_ws_core/build.gradle
new file mode 100644
index 0000000..ccf02ce
--- /dev/null
+++ b/simulation/halsim_ws_core/build.gradle
@@ -0,0 +1,58 @@
+apply plugin: 'cpp'
+apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: ExtraTasks
+
+if (!project.hasProperty('onlylinuxathena')) {
+
+    description = "Core library for WebSocket extensions"
+
+    ext {
+        includeWpiutil = true
+        includeNtCore = true
+        pluginName = 'halsim_ws_core'
+    }
+
+    apply plugin: 'google-test-test-suite'
+
+
+    ext {
+        staticGtestConfigs = [:]
+    }
+
+    staticGtestConfigs["${pluginName}Test"] = []
+    apply from: "${rootDir}/shared/googletest.gradle"
+
+    apply from: "${rootDir}/shared/config.gradle"
+
+    model {
+        components {
+            halsim_ws_core(NativeLibrarySpec) {
+                sources {
+                    cpp {
+                        source {
+                            srcDirs = ['src/main/native/cpp']
+                            includes = ["**/*.cpp"]
+                        }
+                        exportedHeaders {
+                            srcDirs = ["src/main/native/include"]
+                        }
+                    }
+                }
+                binaries.all {
+                    project(':hal').addHalDependency(it, 'shared')
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+
+                }
+                appendDebugPathToBinaries(binaries)
+            }
+        }
+        binaries {
+            all {
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    it.buildable = false
+                    return
+                }
+            }
+        }
+    }
+}

simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp
new file mode 100644
index 0000000..39057a1
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSBaseProvider.h"
+
+namespace wpilibws {
+
+HALSimWSBaseProvider::HALSimWSBaseProvider(const std::string& key,
+                                           const std::string& type)
+    : m_key(key), m_type(type) {}
+
+void HALSimWSBaseProvider::OnNetValueChanged(const wpi::json& json) {
+  // empty
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp
new file mode 100644
index 0000000..bc78202
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+void HALSimWSHalProvider::OnNetworkConnected(
+    std::shared_ptr<HALSimBaseWebSocketConnection> ws) {
+  {
+    // store a weak reference to the websocket object
+    m_ws = ws;
+  }
+
+  RegisterCallbacks();
+}
+
+void HALSimWSHalProvider::OnNetworkDisconnected() { CancelCallbacks(); }
+
+void HALSimWSHalProvider::ProcessHalCallback(const wpi::json& payload) {
+  auto ws = m_ws.lock();
+  if (ws) {
+    wpi::json netValue = {
+        {"type", m_type}, {"device", m_deviceId}, {"data", payload}};
+    ws->OnSimValueChanged(netValue);
+  }
+}
+
+HALSimWSHalChanProvider::HALSimWSHalChanProvider(int32_t channel,
+                                                 const std::string& key,
+                                                 const std::string& type)
+    : HALSimWSHalProvider(key, type), m_channel(channel) {
+  m_deviceId = std::to_string(channel);
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp
new file mode 100644
index 0000000..91a8fa9
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_Analog.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/AnalogInData.h>
+#include <hal/simulation/AnalogOutData.h>
+
+#define REGISTER_AIN(halsim, jsonid, ctype, haltype)                       \
+  HALSIM_RegisterAnalogIn##halsim##Callback(                               \
+      m_channel,                                                           \
+      [](const char* name, void* param, const struct HAL_Value* value) {   \
+        static_cast<HALSimWSProviderAnalogIn*>(param)->ProcessHalCallback( \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});      \
+      },                                                                   \
+      this, true)
+
+#define REGISTER_AIN_ACCUM(halsim, jsonid, ctype, haltype)                 \
+  HALSIM_RegisterAnalogInAccumulator##halsim##Callback(                    \
+      m_channel,                                                           \
+      [](const char* name, void* param, const struct HAL_Value* value) {   \
+        static_cast<HALSimWSProviderAnalogIn*>(param)->ProcessHalCallback( \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});      \
+      },                                                                   \
+      this, true)
+
+#define REGISTER_AOUT(halsim, jsonid, ctype, haltype)                       \
+  HALSIM_RegisterAnalogOut##halsim##Callback(                               \
+      m_channel,                                                            \
+      [](const char* name, void* param, const struct HAL_Value* value) {    \
+        static_cast<HALSimWSProviderAnalogOut*>(param)->ProcessHalCallback( \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});       \
+      },                                                                    \
+      this, true)
+
+namespace wpilibws {
+
+void HALSimWSProviderAnalogIn::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderAnalogIn>("AI", HAL_GetNumAnalogInputs(),
+                                            webRegisterFunc);
+}
+
+HALSimWSProviderAnalogIn::~HALSimWSProviderAnalogIn() { DoCancelCallbacks(); }
+
+void HALSimWSProviderAnalogIn::RegisterCallbacks() {
+  m_initCbKey = REGISTER_AIN(Initialized, "<init", bool, boolean);
+  m_avgbitsCbKey = REGISTER_AIN(AverageBits, "<avg_bits", int32_t, int);
+  m_oversampleCbKey =
+      REGISTER_AIN(OversampleBits, "<oversample_bits", int32_t, int);
+  m_voltageCbKey = REGISTER_AIN(Voltage, ">voltage", double, double);
+
+  m_accumInitCbKey =
+      REGISTER_AIN_ACCUM(Initialized, "<accum_init", bool, boolean);
+  m_accumValueCbKey = REGISTER_AIN_ACCUM(Value, ">accum_value", int64_t,
+                                         long);  // NOLINT(runtime/int)
+  m_accumCountCbKey = REGISTER_AIN_ACCUM(Count, ">accum_count", int64_t,
+                                         long);  // NOLINT(runtime/int)
+  m_accumCenterCbKey =
+      REGISTER_AIN_ACCUM(Center, "<accum_center", int32_t, int);
+  m_accumDeadbandCbKey =
+      REGISTER_AIN_ACCUM(Deadband, "<accum_deadband", int32_t, int);
+}
+
+void HALSimWSProviderAnalogIn::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderAnalogIn::DoCancelCallbacks() {
+  // Cancel callbacks
+  HALSIM_CancelAnalogInInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelAnalogInAverageBitsCallback(m_channel, m_avgbitsCbKey);
+  HALSIM_CancelAnalogInOversampleBitsCallback(m_channel, m_oversampleCbKey);
+  HALSIM_CancelAnalogInVoltageCallback(m_channel, m_voltageCbKey);
+  HALSIM_CancelAnalogInAccumulatorInitializedCallback(m_channel,
+                                                      m_accumInitCbKey);
+  HALSIM_CancelAnalogInAccumulatorValueCallback(m_channel, m_accumValueCbKey);
+  HALSIM_CancelAnalogInAccumulatorCountCallback(m_channel, m_accumCountCbKey);
+  HALSIM_CancelAnalogInAccumulatorCenterCallback(m_channel, m_accumCenterCbKey);
+  HALSIM_CancelAnalogInAccumulatorDeadbandCallback(m_channel,
+                                                   m_accumDeadbandCbKey);
+
+  // Reset callback IDs
+  m_initCbKey = 0;
+  m_avgbitsCbKey = 0;
+  m_oversampleCbKey = 0;
+  m_voltageCbKey = 0;
+  m_accumInitCbKey = 0;
+  m_accumValueCbKey = 0;
+  m_accumCountCbKey = 0;
+  m_accumCenterCbKey = 0;
+  m_accumDeadbandCbKey = 0;
+}
+
+void HALSimWSProviderAnalogIn::OnNetValueChanged(const wpi::json& json) {
+  wpi::json::const_iterator it;
+  if ((it = json.find(">voltage")) != json.end()) {
+    HALSIM_SetAnalogInVoltage(m_channel, it.value());
+  }
+  if ((it = json.find(">accum_value")) != json.end()) {
+    HALSIM_SetAnalogInAccumulatorValue(m_channel, it.value());
+  }
+  if ((it = json.find(">accum_count")) != json.end()) {
+    HALSIM_SetAnalogInAccumulatorCount(m_channel, it.value());
+  }
+}
+
+void HALSimWSProviderAnalogOut::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderAnalogOut>("AO", HAL_GetNumAnalogOutputs(),
+                                             webRegisterFunc);
+}
+
+HALSimWSProviderAnalogOut::~HALSimWSProviderAnalogOut() { CancelCallbacks(); }
+
+void HALSimWSProviderAnalogOut::RegisterCallbacks() {
+  m_initCbKey = REGISTER_AOUT(Initialized, "<init", bool, boolean);
+  m_voltageCbKey = REGISTER_AOUT(Voltage, "<voltage", double, double);
+}
+
+void HALSimWSProviderAnalogOut::CancelCallbacks() {
+  HALSIM_CancelAnalogOutInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelAnalogOutVoltageCallback(m_channel, m_voltageCbKey);
+
+  m_initCbKey = 0;
+  m_voltageCbKey = 0;
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp
new file mode 100644
index 0000000..88fb29f
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_DIO.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/DIOData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                         \
+  HALSIM_RegisterDIO##halsim##Callback(                                  \
+      m_channel,                                                         \
+      [](const char* name, void* param, const struct HAL_Value* value) { \
+        static_cast<HALSimWSProviderDIO*>(param)->ProcessHalCallback(    \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});    \
+      },                                                                 \
+      this, true)
+
+namespace wpilibws {
+
+void HALSimWSProviderDIO::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderDIO>("DIO", HAL_GetNumDigitalChannels(),
+                                       webRegisterFunc);
+}
+
+HALSimWSProviderDIO::~HALSimWSProviderDIO() { DoCancelCallbacks(); }
+
+void HALSimWSProviderDIO::RegisterCallbacks() {
+  m_initCbKey = REGISTER(Initialized, "<init", bool, boolean);
+  m_valueCbKey = REGISTER(Value, "<>value", bool, boolean);
+  m_pulseLengthCbKey = REGISTER(PulseLength, "<pulse_length", double, double);
+  m_inputCbKey = REGISTER(IsInput, "<input", bool, boolean);
+}
+
+void HALSimWSProviderDIO::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderDIO::DoCancelCallbacks() {
+  HALSIM_CancelDIOInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelDIOValueCallback(m_channel, m_valueCbKey);
+  HALSIM_CancelDIOPulseLengthCallback(m_channel, m_pulseLengthCbKey);
+  HALSIM_CancelDIOIsInputCallback(m_channel, m_inputCbKey);
+
+  m_initCbKey = 0;
+  m_valueCbKey = 0;
+  m_pulseLengthCbKey = 0;
+  m_inputCbKey = 0;
+}
+
+void HALSimWSProviderDIO::OnNetValueChanged(const wpi::json& json) {
+  wpi::json::const_iterator it;
+  if ((it = json.find("<>value")) != json.end()) {
+    HALSIM_SetDIOValue(m_channel, static_cast<bool>(it.value()));
+  }
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp
new file mode 100644
index 0000000..b3cec6d
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp
@@ -0,0 +1,169 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_DriverStation.h"
+
+#include <algorithm>
+#include <atomic>
+
+#include <hal/DriverStation.h>
+#include <hal/Extensions.h>
+#include <hal/Ports.h>
+#include <hal/simulation/DriverStationData.h>
+#include <wpi/raw_ostream.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                          \
+  HALSIM_RegisterDriverStation##halsim##Callback(                         \
+      [](const char* name, void* param, const struct HAL_Value* value) {  \
+        static_cast<HALSimWSProviderDriverStation*>(param)                \
+            ->ProcessHalCallback(                                         \
+                {{jsonid, static_cast<ctype>(value->data.v_##haltype)}}); \
+      },                                                                  \
+      this, true)
+
+namespace wpilibws {
+
+std::atomic<bool>* gDSSocketConnected{nullptr};
+
+void HALSimWSProviderDriverStation::Initialize(WSRegisterFunc webRegisterFunc) {
+  static bool registered = false;
+  if (!registered) {
+    registered = true;
+    HAL_RegisterExtensionListener(
+        nullptr, [](void*, const char* name, void* data) {
+          if (wpi::StringRef{name} == "ds_socket") {
+            gDSSocketConnected = static_cast<std::atomic<bool>*>(data);
+          }
+        });
+  }
+  CreateSingleProvider<HALSimWSProviderDriverStation>("DriverStation",
+                                                      webRegisterFunc);
+}
+
+HALSimWSProviderDriverStation::~HALSimWSProviderDriverStation() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderDriverStation::RegisterCallbacks() {
+  m_enabledCbKey = REGISTER(Enabled, ">enabled", bool, boolean);
+  m_autonomousCbKey = REGISTER(Autonomous, ">autonomous", bool, boolean);
+  m_testCbKey = REGISTER(Test, ">test", bool, boolean);
+  m_estopCbKey = REGISTER(EStop, ">estop", bool, boolean);
+  m_fmsCbKey = REGISTER(FmsAttached, ">fms", bool, boolean);
+  m_dsCbKey = REGISTER(DsAttached, ">ds", bool, boolean);
+
+  // Special case for new data, since the HAL_Value is empty
+  m_newDataCbKey = HALSIM_RegisterDriverStationNewDataCallback(
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        static_cast<HALSimWSProviderDriverStation*>(param)->ProcessHalCallback(
+            {{">new_data", true}});
+      },
+      this, true);
+
+  m_allianceCbKey = HALSIM_RegisterDriverStationAllianceStationIdCallback(
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        std::string station;
+        switch (static_cast<HAL_AllianceStationID>(value->data.v_enum)) {
+          case HAL_AllianceStationID_kRed1:
+            station = "red1";
+            break;
+          case HAL_AllianceStationID_kBlue1:
+            station = "blue1";
+            break;
+          case HAL_AllianceStationID_kRed2:
+            station = "red2";
+            break;
+          case HAL_AllianceStationID_kBlue2:
+            station = "blue2";
+            break;
+          case HAL_AllianceStationID_kRed3:
+            station = "red3";
+            break;
+          case HAL_AllianceStationID_kBlue3:
+            station = "blue3";
+            break;
+        }
+        static_cast<HALSimWSProviderDriverStation*>(param)->ProcessHalCallback(
+            {{">station", station}});
+      },
+      this, true);
+
+  m_matchTimeCbKey = REGISTER(MatchTime, "<match_time", double, double);
+}
+
+void HALSimWSProviderDriverStation::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderDriverStation::DoCancelCallbacks() {
+  HALSIM_CancelDriverStationEnabledCallback(m_enabledCbKey);
+  HALSIM_CancelDriverStationAutonomousCallback(m_autonomousCbKey);
+  HALSIM_CancelDriverStationTestCallback(m_testCbKey);
+  HALSIM_CancelDriverStationEStopCallback(m_estopCbKey);
+  HALSIM_CancelDriverStationFmsAttachedCallback(m_fmsCbKey);
+  HALSIM_CancelDriverStationDsAttachedCallback(m_dsCbKey);
+  HALSIM_CancelDriverStationNewDataCallback(m_newDataCbKey);
+  HALSIM_CancelDriverStationAllianceStationIdCallback(m_allianceCbKey);
+  HALSIM_CancelDriverStationMatchTimeCallback(m_matchTimeCbKey);
+
+  m_enabledCbKey = 0;
+  m_autonomousCbKey = 0;
+  m_testCbKey = 0;
+  m_estopCbKey = 0;
+  m_fmsCbKey = 0;
+  m_dsCbKey = 0;
+  m_newDataCbKey = 0;
+  m_allianceCbKey = 0;
+  m_matchTimeCbKey = 0;
+}
+
+void HALSimWSProviderDriverStation::OnNetValueChanged(const wpi::json& json) {
+  // ignore if DS connected
+  if (gDSSocketConnected && *gDSSocketConnected) return;
+
+  wpi::json::const_iterator it;
+  if ((it = json.find(">enabled")) != json.end()) {
+    HALSIM_SetDriverStationEnabled(it.value());
+  }
+  if ((it = json.find(">autonomous")) != json.end()) {
+    HALSIM_SetDriverStationAutonomous(it.value());
+  }
+  if ((it = json.find(">test")) != json.end()) {
+    HALSIM_SetDriverStationTest(it.value());
+  }
+  if ((it = json.find(">estop")) != json.end()) {
+    HALSIM_SetDriverStationEStop(it.value());
+  }
+  if ((it = json.find(">fms")) != json.end()) {
+    HALSIM_SetDriverStationFmsAttached(it.value());
+  }
+  if ((it = json.find(">ds")) != json.end()) {
+    HALSIM_SetDriverStationDsAttached(it.value());
+  }
+
+  if ((it = json.find(">station")) != json.end()) {
+    auto& station = it.value().get_ref<const std::string&>();
+    if (station == "red1") {
+      HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kRed1);
+    } else if (station == "red2") {
+      HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kRed2);
+    } else if (station == "red3") {
+      HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kRed3);
+    } else if (station == "blue1") {
+      HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kBlue1);
+    } else if (station == "blue2") {
+      HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kBlue2);
+    } else if (station == "blue3") {
+      HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kBlue3);
+    }
+  }
+
+  // Only notify usercode if we get the new data message
+  if ((it = json.find(">new_data")) != json.end()) {
+    HALSIM_NotifyDriverStationNewData();
+  }
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
new file mode 100644
index 0000000..58d47d0
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_Encoder.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/EncoderData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                          \
+  HALSIM_RegisterEncoder##halsim##Callback(                               \
+      m_channel,                                                          \
+      [](const char* name, void* param, const struct HAL_Value* value) {  \
+        static_cast<HALSimWSProviderEncoder*>(param)->ProcessHalCallback( \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});     \
+      },                                                                  \
+      this, true)
+
+namespace wpilibws {
+
+void HALSimWSProviderEncoder::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderEncoder>("Encoder", HAL_GetNumEncoders(),
+                                           webRegisterFunc);
+}
+
+HALSimWSProviderEncoder::~HALSimWSProviderEncoder() { DoCancelCallbacks(); }
+
+void HALSimWSProviderEncoder::RegisterCallbacks() {
+  // Special case for initialization since we will need to send
+  // the digital channels down the line as well
+  m_initCbKey = HALSIM_RegisterEncoderInitializedCallback(
+      m_channel,
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        auto provider = static_cast<HALSimWSProviderEncoder*>(param);
+        bool init = static_cast<bool>(value->data.v_boolean);
+
+        wpi::json payload = {{"<init", init}};
+
+        if (init) {
+          payload["<channel_a"] =
+              HALSIM_GetEncoderDigitalChannelA(provider->GetChannel());
+          payload["<channel_b"] =
+              HALSIM_GetEncoderDigitalChannelB(provider->GetChannel());
+        }
+
+        provider->ProcessHalCallback(payload);
+      },
+      this, true);
+  m_countCbKey = REGISTER(Count, ">count", int32_t, int);
+  m_periodCbKey = REGISTER(Period, ">period", double, double);
+  m_resetCbKey = REGISTER(Reset, "<reset", bool, boolean);
+  m_reverseDirectionCbKey =
+      REGISTER(ReverseDirection, "<reverse_direction", bool, boolean);
+  m_samplesCbKey = REGISTER(SamplesToAverage, "<samples_to_avg", int32_t, int);
+}
+
+void HALSimWSProviderEncoder::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderEncoder::DoCancelCallbacks() {
+  HALSIM_CancelEncoderInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelEncoderCountCallback(m_channel, m_countCbKey);
+  HALSIM_CancelEncoderPeriodCallback(m_channel, m_periodCbKey);
+  HALSIM_CancelEncoderResetCallback(m_channel, m_resetCbKey);
+  HALSIM_CancelEncoderReverseDirectionCallback(m_channel,
+                                               m_reverseDirectionCbKey);
+  HALSIM_CancelEncoderSamplesToAverageCallback(m_channel, m_samplesCbKey);
+
+  m_initCbKey = 0;
+  m_countCbKey = 0;
+  m_periodCbKey = 0;
+  m_resetCbKey = 0;
+  m_reverseDirectionCbKey = 0;
+  m_samplesCbKey = 0;
+}
+
+void HALSimWSProviderEncoder::OnNetValueChanged(const wpi::json& json) {
+  wpi::json::const_iterator it;
+  if ((it = json.find(">count")) != json.end()) {
+    HALSIM_SetEncoderCount(m_channel, it.value());
+  }
+  if ((it = json.find(">period")) != json.end()) {
+    HALSIM_SetEncoderPeriod(m_channel, it.value());
+  }
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp
new file mode 100644
index 0000000..6f3f27b
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_Joystick.h"
+
+#include <atomic>
+
+#include <hal/Ports.h>
+#include <hal/simulation/DriverStationData.h>
+
+namespace wpilibws {
+
+extern std::atomic<bool>* gDSSocketConnected;
+
+void HALSimWSProviderJoystick::Initialize(WSRegisterFunc webregisterFunc) {
+  CreateProviders<HALSimWSProviderJoystick>("Joystick", HAL_kMaxJoysticks,
+                                            webregisterFunc);
+}
+
+HALSimWSProviderJoystick::~HALSimWSProviderJoystick() { DoCancelCallbacks(); }
+
+void HALSimWSProviderJoystick::RegisterCallbacks() {
+  m_dsNewDataCbKey = HALSIM_RegisterDriverStationNewDataCallback(
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        auto provider = static_cast<HALSimWSProviderJoystick*>(param);
+
+        // Grab all joystick data and send it at once
+        wpi::json payload;
+
+        // Axes data
+        HAL_JoystickAxes axes{};
+        std::vector<double> axesValues;
+        HALSIM_GetJoystickAxes(provider->GetChannel(), &axes);
+
+        for (int i = 0; i < axes.count; i++) {
+          axesValues.push_back(axes.axes[i]);
+        }
+
+        // POVs data
+        HAL_JoystickPOVs povs{};
+        std::vector<int16_t> povsValues;
+        HALSIM_GetJoystickPOVs(provider->GetChannel(), &povs);
+
+        for (int i = 0; i < povs.count; i++) {
+          povsValues.push_back(povs.povs[i]);
+        }
+
+        // Button data
+        HAL_JoystickButtons buttons{};
+        std::vector<bool> buttonsValues;
+
+        for (int i = 0; i < buttons.count; i++) {
+          buttonsValues.push_back(((buttons.buttons >> i) & 0x1) == 1);
+        }
+
+        payload[">axes"] = axesValues;
+        payload[">povs"] = povsValues;
+        payload[">buttons"] = buttonsValues;
+
+        provider->ProcessHalCallback(payload);
+      },
+      this, true);
+}
+
+void HALSimWSProviderJoystick::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderJoystick::DoCancelCallbacks() {
+  HALSIM_CancelDriverStationNewDataCallback(m_dsNewDataCbKey);
+
+  m_dsNewDataCbKey = 0;
+}
+
+void HALSimWSProviderJoystick::OnNetValueChanged(const wpi::json& json) {
+  // ignore if DS connected
+  if (gDSSocketConnected && *gDSSocketConnected) return;
+
+  wpi::json::const_iterator it;
+  if ((it = json.find(">axes")) != json.end()) {
+    HAL_JoystickAxes axes{};
+    axes.count =
+        std::min(it.value().size(), (wpi::json::size_type)HAL_kMaxJoystickAxes);
+    for (int i = 0; i < axes.count; i++) {
+      axes.axes[i] = it.value()[i];
+    }
+
+    HALSIM_SetJoystickAxes(m_channel, &axes);
+  }
+
+  if ((it = json.find(">buttons")) != json.end()) {
+    HAL_JoystickButtons buttons{};
+    buttons.count = std::min(it.value().size(), (wpi::json::size_type)32);
+    for (int i = 0; i < buttons.count; i++) {
+      if (it.value()[i]) {
+        buttons.buttons |= 1 << (i - 1);
+      }
+    }
+
+    HALSIM_SetJoystickButtons(m_channel, &buttons);
+  }
+
+  if ((it = json.find(">povs")) != json.end()) {
+    HAL_JoystickPOVs povs{};
+    povs.count =
+        std::min(it.value().size(), (wpi::json::size_type)HAL_kMaxJoystickPOVs);
+    for (int i = 0; i < povs.count; i++) {
+      povs.povs[i] = it.value()[i];
+    }
+
+    HALSIM_SetJoystickPOVs(m_channel, &povs);
+  }
+  // We won't send any updates to user code until the new data message
+  // is received by the driver station provider
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp
new file mode 100644
index 0000000..cd0f4f4
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_PWM.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/PWMData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                         \
+  HALSIM_RegisterPWM##halsim##Callback(                                  \
+      m_channel,                                                         \
+      [](const char* name, void* param, const struct HAL_Value* value) { \
+        static_cast<HALSimWSProviderPWM*>(param)->ProcessHalCallback(    \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});    \
+      },                                                                 \
+      this, true)
+namespace wpilibws {
+
+void HALSimWSProviderPWM::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderPWM>("PWM", HAL_GetNumPWMChannels(),
+                                       webRegisterFunc);
+}
+
+HALSimWSProviderPWM::~HALSimWSProviderPWM() { DoCancelCallbacks(); }
+
+void HALSimWSProviderPWM::RegisterCallbacks() {
+  m_initCbKey = REGISTER(Initialized, "<init", bool, boolean);
+  m_speedCbKey = REGISTER(Speed, "<speed", double, double);
+  m_positionCbKey = REGISTER(Position, "<position", double, double);
+  m_rawCbKey = REGISTER(RawValue, "<raw", int32_t, int);
+  m_periodScaleCbKey = REGISTER(PeriodScale, "<period_scale", int32_t, int);
+  m_zeroLatchCbKey = REGISTER(ZeroLatch, "<zero_latch", bool, boolean);
+}
+
+void HALSimWSProviderPWM::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderPWM::DoCancelCallbacks() {
+  HALSIM_CancelPWMInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelPWMSpeedCallback(m_channel, m_speedCbKey);
+  HALSIM_CancelPWMPositionCallback(m_channel, m_positionCbKey);
+  HALSIM_CancelPWMRawValueCallback(m_channel, m_rawCbKey);
+  HALSIM_CancelPWMPeriodScaleCallback(m_channel, m_periodScaleCbKey);
+  HALSIM_CancelPWMZeroLatchCallback(m_channel, m_zeroLatchCbKey);
+
+  m_initCbKey = 0;
+  m_speedCbKey = 0;
+  m_positionCbKey = 0;
+  m_rawCbKey = 0;
+  m_periodScaleCbKey = 0;
+  m_zeroLatchCbKey = 0;
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp
new file mode 100644
index 0000000..e51658f
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_Relay.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/RelayData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                         \
+  HALSIM_RegisterRelay##halsim##Callback(                                \
+      m_channel,                                                         \
+      [](const char* name, void* param, const struct HAL_Value* value) { \
+        static_cast<HALSimWSProviderRelay*>(param)->ProcessHalCallback(  \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});    \
+      },                                                                 \
+      this, true)
+
+namespace wpilibws {
+void HALSimWSProviderRelay::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderRelay>("Relay", HAL_GetNumRelayHeaders(),
+                                         webRegisterFunc);
+}
+
+HALSimWSProviderRelay::~HALSimWSProviderRelay() { DoCancelCallbacks(); }
+
+void HALSimWSProviderRelay::RegisterCallbacks() {
+  m_initFwdCbKey = REGISTER(InitializedForward, "<init_fwd", bool, boolean);
+  m_initRevCbKey = REGISTER(InitializedReverse, "<init_rev", bool, boolean);
+  m_fwdCbKey = REGISTER(Forward, "<fwd", bool, boolean);
+  m_revCbKey = REGISTER(Reverse, "<rev", bool, boolean);
+}
+
+void HALSimWSProviderRelay::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderRelay::DoCancelCallbacks() {
+  HALSIM_CancelRelayInitializedForwardCallback(m_channel, m_initFwdCbKey);
+  HALSIM_CancelRelayInitializedReverseCallback(m_channel, m_initRevCbKey);
+  HALSIM_CancelRelayForwardCallback(m_channel, m_fwdCbKey);
+  HALSIM_CancelRelayReverseCallback(m_channel, m_revCbKey);
+
+  m_initFwdCbKey = 0;
+  m_initRevCbKey = 0;
+  m_fwdCbKey = 0;
+  m_revCbKey = 0;
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp
new file mode 100644
index 0000000..42e2cc8
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp
@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_RoboRIO.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/RoboRioData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                          \
+  HALSIM_RegisterRoboRio##halsim##Callback(                               \
+      [](const char* name, void* param, const struct HAL_Value* value) {  \
+        static_cast<HALSimWSProviderRoboRIO*>(param)->ProcessHalCallback( \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});     \
+      },                                                                  \
+      this, true)
+
+namespace wpilibws {
+
+void HALSimWSProviderRoboRIO::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateSingleProvider<HALSimWSProviderRoboRIO>("RoboRIO", webRegisterFunc);
+}
+
+HALSimWSProviderRoboRIO::~HALSimWSProviderRoboRIO() { DoCancelCallbacks(); }
+
+void HALSimWSProviderRoboRIO::RegisterCallbacks() {
+  m_fpgaCbKey = REGISTER(FPGAButton, ">fpga_button", bool, boolean);
+  m_vinVoltageCbKey = REGISTER(VInVoltage, ">vin_voltage", double, double);
+  m_vinCurrentCbKey = REGISTER(VInCurrent, ">vin_current", double, double);
+
+  m_6vVoltageCbKey = REGISTER(UserVoltage6V, ">6v_voltage", double, double);
+  m_6vCurrentCbKey = REGISTER(UserCurrent6V, ">6v_current", double, double);
+  m_6vActiveCbKey = REGISTER(UserActive6V, ">6v_active", bool, boolean);
+  m_6vFaultsCbKey = REGISTER(UserFaults6V, ">6v_faults", int32_t, int);
+
+  m_5vVoltageCbKey = REGISTER(UserVoltage5V, ">5v_voltage", double, double);
+  m_5vCurrentCbKey = REGISTER(UserCurrent5V, ">5v_current", double, double);
+  m_5vActiveCbKey = REGISTER(UserActive5V, ">5v_active", bool, boolean);
+  m_5vFaultsCbKey = REGISTER(UserFaults5V, ">5v_faults", int32_t, int);
+
+  m_3v3VoltageCbKey = REGISTER(UserVoltage3V3, ">3v3_voltage", double, double);
+  m_3v3CurrentCbKey = REGISTER(UserCurrent3V3, ">3v3_current", double, double);
+  m_3v3ActiveCbKey = REGISTER(UserActive3V3, ">3v3_active", bool, boolean);
+  m_3v3FaultsCbKey = REGISTER(UserFaults3V3, ">3v3_faults", int32_t, int);
+}
+
+void HALSimWSProviderRoboRIO::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderRoboRIO::DoCancelCallbacks() {
+  HALSIM_CancelRoboRioFPGAButtonCallback(m_fpgaCbKey);
+  HALSIM_CancelRoboRioVInVoltageCallback(m_vinVoltageCbKey);
+  HALSIM_CancelRoboRioVInCurrentCallback(m_vinCurrentCbKey);
+
+  HALSIM_CancelRoboRioUserVoltage6VCallback(m_6vVoltageCbKey);
+  HALSIM_CancelRoboRioUserCurrent6VCallback(m_6vCurrentCbKey);
+  HALSIM_CancelRoboRioUserActive6VCallback(m_6vActiveCbKey);
+  HALSIM_CancelRoboRioUserFaults6VCallback(m_6vFaultsCbKey);
+
+  HALSIM_CancelRoboRioUserVoltage5VCallback(m_5vVoltageCbKey);
+  HALSIM_CancelRoboRioUserCurrent5VCallback(m_5vCurrentCbKey);
+  HALSIM_CancelRoboRioUserActive5VCallback(m_5vActiveCbKey);
+  HALSIM_CancelRoboRioUserFaults5VCallback(m_5vFaultsCbKey);
+
+  HALSIM_CancelRoboRioUserVoltage3V3Callback(m_3v3VoltageCbKey);
+  HALSIM_CancelRoboRioUserCurrent3V3Callback(m_3v3CurrentCbKey);
+  HALSIM_CancelRoboRioUserActive3V3Callback(m_3v3ActiveCbKey);
+  HALSIM_CancelRoboRioUserFaults3V3Callback(m_3v3FaultsCbKey);
+
+  m_fpgaCbKey = 0;
+  m_vinVoltageCbKey = 0;
+  m_vinCurrentCbKey = 0;
+  m_6vActiveCbKey = 0;
+  m_6vCurrentCbKey = 0;
+  m_6vFaultsCbKey = 0;
+  m_6vVoltageCbKey = 0;
+  m_5vActiveCbKey = 0;
+  m_5vCurrentCbKey = 0;
+  m_5vFaultsCbKey = 0;
+  m_5vVoltageCbKey = 0;
+  m_3v3ActiveCbKey = 0;
+  m_3v3CurrentCbKey = 0;
+  m_3v3FaultsCbKey = 0;
+  m_3v3VoltageCbKey = 0;
+}
+
+void HALSimWSProviderRoboRIO::OnNetValueChanged(const wpi::json& json) {
+  wpi::json::const_iterator it;
+  if ((it = json.find(">fpga_button")) != json.end()) {
+    HALSIM_SetRoboRioFPGAButton(static_cast<bool>(it.value()));
+  }
+
+  if ((it = json.find(">vin_voltage")) != json.end()) {
+    HALSIM_SetRoboRioVInVoltage(it.value());
+  }
+  if ((it = json.find(">vin_current")) != json.end()) {
+    HALSIM_SetRoboRioVInCurrent(it.value());
+  }
+
+  if ((it = json.find(">6v_voltage")) != json.end()) {
+    HALSIM_SetRoboRioUserVoltage6V(it.value());
+  }
+  if ((it = json.find(">6v_current")) != json.end()) {
+    HALSIM_SetRoboRioUserCurrent6V(it.value());
+  }
+  if ((it = json.find(">6v_active")) != json.end()) {
+    HALSIM_SetRoboRioUserActive6V(static_cast<bool>(it.value()));
+  }
+  if ((it = json.find(">6v_faults")) != json.end()) {
+    HALSIM_SetRoboRioUserFaults6V(it.value());
+  }
+
+  if ((it = json.find(">5v_voltage")) != json.end()) {
+    HALSIM_SetRoboRioUserVoltage5V(it.value());
+  }
+  if ((it = json.find(">5v_current")) != json.end()) {
+    HALSIM_SetRoboRioUserCurrent5V(it.value());
+  }
+  if ((it = json.find(">5v_active")) != json.end()) {
+    HALSIM_SetRoboRioUserActive5V(static_cast<bool>(it.value()));
+  }
+  if ((it = json.find(">5v_faults")) != json.end()) {
+    HALSIM_SetRoboRioUserFaults5V(it.value());
+  }
+
+  if ((it = json.find(">3v3_voltage")) != json.end()) {
+    HALSIM_SetRoboRioUserVoltage3V3(it.value());
+  }
+  if ((it = json.find(">3v3_current")) != json.end()) {
+    HALSIM_SetRoboRioUserCurrent3V3(it.value());
+  }
+  if ((it = json.find(">3v3_active")) != json.end()) {
+    HALSIM_SetRoboRioUserActive3V3(static_cast<bool>(it.value()));
+  }
+  if ((it = json.find(">3v3_faults")) != json.end()) {
+    HALSIM_SetRoboRioUserFaults3V3(it.value());
+  }
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp
new file mode 100644
index 0000000..ee8fded
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp
@@ -0,0 +1,196 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_SimDevice.h"
+
+#include <hal/Ports.h>
+
+namespace wpilibws {
+
+HALSimWSProviderSimDevice::~HALSimWSProviderSimDevice() { CancelCallbacks(); }
+
+void HALSimWSProviderSimDevice::OnNetworkConnected(
+    std::shared_ptr<HALSimBaseWebSocketConnection> ws) {
+  auto storedWS = m_ws.lock();
+
+  if (ws == storedWS) {
+    return;
+  }
+
+  // Otherwise, run the disconnection code first so we get
+  // back to a clean slate (only if the stored websocket is
+  // not null)
+  if (storedWS) {
+    OnNetworkDisconnected();
+  }
+
+  m_ws = ws;
+
+  m_simValueCreatedCbKey = HALSIM_RegisterSimValueCreatedCallback(
+      m_handle, this, HALSimWSProviderSimDevice::OnValueCreatedStatic, 1);
+}
+
+void HALSimWSProviderSimDevice::OnNetworkDisconnected() {
+  // Cancel all callbacks
+  CancelCallbacks();
+
+  m_ws.reset();
+}
+
+void HALSimWSProviderSimDevice::CancelCallbacks() {
+  HALSIM_CancelSimValueCreatedCallback(m_simValueCreatedCbKey);
+
+  m_simValueCreatedCbKey = 0;
+
+  for (auto& kv : m_simValueChangedCbKeys) {
+    HALSIM_CancelSimValueChangedCallback(kv.getValue());
+  }
+
+  m_simValueChangedCbKeys.clear();
+}
+
+void HALSimWSProviderSimDevice::OnNetValueChanged(const wpi::json& json) {
+  auto it = json.cbegin();
+  auto end = json.cend();
+
+  std::shared_lock lock(m_vhLock);
+  for (; it != end; ++it) {
+    auto vd = m_valueHandles.find(it.key());
+    if (vd != m_valueHandles.end()) {
+      HAL_Value value;
+      value.type = vd->second->valueType;
+      switch (value.type) {
+        case HAL_BOOLEAN:
+          value.data.v_boolean = static_cast<bool>(it.value()) ? 1 : 0;
+          break;
+        case HAL_DOUBLE:
+          value.data.v_double = it.value();
+          break;
+        case HAL_ENUM:
+          value.data.v_enum = it.value();
+          break;
+        case HAL_INT:
+          value.data.v_int = it.value();
+          break;
+        case HAL_LONG:
+          value.data.v_long = it.value();
+          break;
+        default:
+          break;
+      }
+
+      HAL_SetSimValue(vd->second->handle, &value);
+    }
+  }
+}
+
+void HALSimWSProviderSimDevice::OnValueCreated(const char* name,
+                                               HAL_SimValueHandle handle,
+                                               HAL_Bool readonly,
+                                               const struct HAL_Value* value) {
+  wpi::Twine key = wpi::Twine(readonly ? "<" : "<>") + name;
+  auto data = std::make_unique<SimDeviceValueData>();
+  data->device = this;
+  data->handle = handle;
+  data->key = key.str();
+  data->valueType = value->type;
+
+  auto param = data.get();
+
+  {
+    std::unique_lock lock(m_vhLock);
+    m_valueHandles[data->key] = std::move(data);
+  }
+
+  int32_t cbKey = HALSIM_RegisterSimValueChangedCallback(
+      handle, param, HALSimWSProviderSimDevice::OnValueChangedStatic, true);
+
+  m_simValueChangedCbKeys[key.str()] = cbKey;
+}
+
+void HALSimWSProviderSimDevice::OnValueChanged(SimDeviceValueData* valueData,
+                                               const struct HAL_Value* value) {
+  auto ws = m_ws.lock();
+  if (ws) {
+    switch (value->type) {
+      case HAL_BOOLEAN:
+        ProcessHalCallback({{valueData->key, value->data.v_boolean}});
+        break;
+      case HAL_DOUBLE:
+        ProcessHalCallback({{valueData->key, value->data.v_double}});
+        break;
+      case HAL_ENUM:
+        ProcessHalCallback({{valueData->key, value->data.v_enum}});
+        break;
+      case HAL_INT:
+        ProcessHalCallback({{valueData->key, value->data.v_int}});
+        break;
+      case HAL_LONG:
+        ProcessHalCallback({{valueData->key, value->data.v_long}});
+        break;
+      default:
+        break;
+    }
+  }
+}
+
+void HALSimWSProviderSimDevice::ProcessHalCallback(const wpi::json& payload) {
+  auto ws = m_ws.lock();
+  if (ws) {
+    wpi::json netValue = {
+        {"type", "SimDevices"}, {"device", m_deviceId}, {"data", payload}};
+    ws->OnSimValueChanged(netValue);
+  }
+}
+
+HALSimWSProviderSimDevices::~HALSimWSProviderSimDevices() { CancelCallbacks(); }
+
+void HALSimWSProviderSimDevices::DeviceCreatedCallback(
+    const char* name, HAL_SimDeviceHandle handle) {
+  auto key = (wpi::Twine("SimDevices/") + name).str();
+  auto dev = std::make_shared<HALSimWSProviderSimDevice>(
+      handle, key, wpi::Twine(name).str());
+  m_providers.Add(key, dev);
+
+  if (m_ws) {
+    m_exec->Call([this, dev]() { dev->OnNetworkConnected(GetWSConnection()); });
+  }
+}
+
+void HALSimWSProviderSimDevices::DeviceFreedCallback(
+    const char* name, HAL_SimDeviceHandle handle) {
+  m_providers.Delete(name);
+}
+
+void HALSimWSProviderSimDevices::Initialize(wpi::uv::Loop& loop) {
+  m_deviceCreatedCbKey = HALSIM_RegisterSimDeviceCreatedCallback(
+      "", this, HALSimWSProviderSimDevices::DeviceCreatedCallbackStatic, 1);
+  m_deviceFreedCbKey = HALSIM_RegisterSimDeviceFreedCallback(
+      "", this, HALSimWSProviderSimDevices::DeviceFreedCallbackStatic);
+
+  m_exec = UvExecFn::Create(loop, [](auto out, LoopFn func) {
+    func();
+    out.set_value();
+  });
+}
+
+void HALSimWSProviderSimDevices::CancelCallbacks() {
+  HALSIM_CancelSimDeviceCreatedCallback(m_deviceCreatedCbKey);
+  HALSIM_CancelSimDeviceFreedCallback(m_deviceFreedCbKey);
+
+  m_deviceCreatedCbKey = 0;
+  m_deviceFreedCbKey = 0;
+}
+
+void HALSimWSProviderSimDevices::OnNetworkConnected(
+    std::shared_ptr<HALSimBaseWebSocketConnection> hws) {
+  m_ws = hws;
+}
+
+void HALSimWSProviderSimDevices::OnNetworkDisconnected() { m_ws = nullptr; }
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp

diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp
new file mode 100644
index 0000000..ce4fd79
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WSProvider_dPWM.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/DigitalPWMData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                             \
+  HALSIM_RegisterDigitalPWM##halsim##Callback(                               \
+      m_channel,                                                             \
+      [](const char* name, void* param, const struct HAL_Value* value) {     \
+        static_cast<HALSimWSProviderDigitalPWM*>(param)->ProcessHalCallback( \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});        \
+      },                                                                     \
+      this, true)
+
+namespace wpilibws {
+
+void HALSimWSProviderDigitalPWM::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderDigitalPWM>(
+      "dPWM", HAL_GetNumDigitalPWMOutputs(), webRegisterFunc);
+}
+
+HALSimWSProviderDigitalPWM::~HALSimWSProviderDigitalPWM() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderDigitalPWM::RegisterCallbacks() {
+  m_initCbKey = REGISTER(Initialized, "<init", bool, boolean);
+  m_dutyCycleCbKey = REGISTER(DutyCycle, "<duty_cycle", double, double);
+  m_pinCbKey = REGISTER(Pin, "<dio_pin", int32_t, int);
+}
+
+void HALSimWSProviderDigitalPWM::CancelCallbacks() { DoCancelCallbacks(); }
+
+void HALSimWSProviderDigitalPWM::DoCancelCallbacks() {
+  HALSIM_CancelDigitalPWMInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelDigitalPWMDutyCycleCallback(m_channel, m_dutyCycleCbKey);
+  HALSIM_CancelDigitalPWMPinCallback(m_channel, m_pinCbKey);
+
+  m_initCbKey = 0;
+  m_dutyCycleCbKey = 0;
+  m_pinCbKey = 0;
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h b/simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h
new file mode 100644
index 0000000..9f01100
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/json.h>
+
+namespace wpilibws {
+
+class HALSimBaseWebSocketConnection {
+ public:
+  virtual void OnSimValueChanged(const wpi::json& msg) = 0;
+
+ protected:
+  virtual ~HALSimBaseWebSocketConnection() = default;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h b/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h
new file mode 100644
index 0000000..46873b7
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <wpi/json.h>
+
+#include "HALSimBaseWebSocketConnection.h"
+
+namespace wpilibws {
+
+class HALSimWSBaseProvider {
+ public:
+  explicit HALSimWSBaseProvider(const std::string& key,
+                                const std::string& type = "");
+  virtual ~HALSimWSBaseProvider() {}
+
+  HALSimWSBaseProvider(const HALSimWSBaseProvider&) = delete;
+  HALSimWSBaseProvider& operator=(const HALSimWSBaseProvider&) = delete;
+
+  // Called when the websocket connects. This will cause providers
+  // to register their HAL callbacks
+  virtual void OnNetworkConnected(
+      std::shared_ptr<HALSimBaseWebSocketConnection> ws) = 0;
+
+  // Called when the websocket disconnects. This will cause provider
+  // to cancel their HAL callbacks
+  virtual void OnNetworkDisconnected() = 0;
+
+  // network -> sim
+  virtual void OnNetValueChanged(const wpi::json& json);
+
+  const std::string GetDeviceType() { return m_type; }
+  const std::string GetDeviceId() { return m_deviceId; }
+
+ protected:
+  // sim -> network
+  std::weak_ptr<HALSimBaseWebSocketConnection> m_ws;
+  std::string m_key;
+
+  std::string m_type;
+  std::string m_deviceId = "";
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h
new file mode 100644
index 0000000..be32607
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <hal/simulation/NotifyListener.h>
+#include <wpi/json.h>
+#include <wpi/mutex.h>
+
+#include "WSBaseProvider.h"
+
+namespace wpilibws {
+
+typedef void (*HALCbRegisterIndexedFunc)(int32_t index,
+                                         HAL_NotifyCallback callback,
+                                         void* param, HAL_Bool initialNotify);
+typedef void (*HALCbRegisterSingleFunc)(HAL_NotifyCallback callback,
+                                        void* param, HAL_Bool initialNotify);
+
+// provider generates diffs based on values
+class HALSimWSHalProvider : public HALSimWSBaseProvider {
+ public:
+  using HALSimWSBaseProvider::HALSimWSBaseProvider;
+
+  void OnNetworkConnected(std::shared_ptr<HALSimBaseWebSocketConnection> ws);
+  void OnNetworkDisconnected();
+
+  void ProcessHalCallback(const wpi::json& payload);
+
+ protected:
+  virtual void RegisterCallbacks() = 0;
+  virtual void CancelCallbacks() = 0;
+};
+
+// provider generates per-channel diffs
+class HALSimWSHalChanProvider : public HALSimWSHalProvider {
+ public:
+  explicit HALSimWSHalChanProvider(int32_t channel, const std::string& key,
+                                   const std::string& type);
+
+  int32_t GetChannel() { return m_channel; }
+
+ protected:
+  int32_t m_channel;
+};
+
+using WSRegisterFunc = std::function<void(
+    const std::string&, std::shared_ptr<HALSimWSBaseProvider>)>;
+
+template <typename T>
+void CreateProviders(const std::string& prefix, int32_t numChannels,
+                     WSRegisterFunc webRegisterFunc);
+
+template <typename T>
+void CreateSingleProvider(const std::string& key,
+                          WSRegisterFunc webRegisterFunc);
+
+#include "WSHalProviders.inl"
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl
new file mode 100644
index 0000000..6bcdcdd
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <utility>
+
+template <typename T>
+void CreateProviders(const std::string& prefix, int numChannels,
+                     WSRegisterFunc webRegisterFunc) {
+  for (int32_t i = 0; i < numChannels; i++) {
+    auto key = (prefix + "/" + wpi::Twine(i)).str();
+    auto ptr = std::make_unique<T>(i, key, prefix);
+    webRegisterFunc(key, std::move(ptr));
+  }
+}
+
+template <typename T>
+void CreateSingleProvider(const std::string& key,
+                          WSRegisterFunc webRegisterFunc) {
+  auto ptr = std::make_unique<T>(key, key);
+  webRegisterFunc(key, std::move(ptr));
+}

simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h b/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h
new file mode 100644
index 0000000..7cd1240
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <shared_mutex>
+
+#include <wpi/StringMap.h>
+
+#include "WSBaseProvider.h"
+
+namespace wpilibws {
+
+class ProviderContainer {
+ public:
+  using ProviderPtr = std::shared_ptr<HALSimWSBaseProvider>;
+  using IterFn = std::function<void(ProviderPtr)>;
+
+  ProviderContainer() {}
+
+  ProviderContainer(const ProviderContainer&) = delete;
+  ProviderContainer& operator=(const ProviderContainer&) = delete;
+
+  void Add(wpi::StringRef key, std::shared_ptr<HALSimWSBaseProvider> provider) {
+    std::unique_lock lock(m_mutex);
+    m_providers[key] = provider;
+  }
+
+  void Delete(wpi::StringRef key) {
+    std::unique_lock lock(m_mutex);
+    m_providers.erase(key);
+  }
+
+  void ForEach(IterFn fn) {
+    std::shared_lock lock(m_mutex);
+    for (auto& kv : m_providers) {
+      fn(kv.getValue());
+    }
+  }
+
+  ProviderPtr Get(wpi::StringRef key) {
+    std::shared_lock lock(m_mutex);
+    auto fiter = m_providers.find(key);
+    return fiter != m_providers.end() ? fiter->second : ProviderPtr();
+  }
+
+ private:
+  std::shared_mutex m_mutex;
+  wpi::StringMap<std::shared_ptr<HALSimWSBaseProvider>> m_providers;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h
new file mode 100644
index 0000000..8ebc235
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderAnalogIn : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderAnalogIn();
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_avgbitsCbKey = 0;
+  int32_t m_oversampleCbKey = 0;
+  int32_t m_voltageCbKey = 0;
+  int32_t m_accumInitCbKey = 0;
+  int32_t m_accumValueCbKey = 0;
+  int32_t m_accumCountCbKey = 0;
+  int32_t m_accumCenterCbKey = 0;
+  int32_t m_accumDeadbandCbKey = 0;
+};
+
+class HALSimWSProviderAnalogOut : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderAnalogOut();
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_voltageCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h
new file mode 100644
index 0000000..8d6da7e
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderDIO : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderDIO();
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_valueCbKey = 0;
+  int32_t m_pulseLengthCbKey = 0;
+  int32_t m_inputCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h
new file mode 100644
index 0000000..c61419c
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderDriverStation : public HALSimWSHalProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalProvider::HALSimWSHalProvider;
+  ~HALSimWSProviderDriverStation();
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_enabledCbKey = 0;
+  int32_t m_autonomousCbKey = 0;
+  int32_t m_testCbKey = 0;
+  int32_t m_estopCbKey = 0;
+  int32_t m_fmsCbKey = 0;
+  int32_t m_dsCbKey = 0;
+  int32_t m_allianceCbKey = 0;
+  int32_t m_matchTimeCbKey = 0;
+  int32_t m_newDataCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
new file mode 100644
index 0000000..882e447
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderEncoder : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderEncoder();
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_countCbKey = 0;
+  int32_t m_periodCbKey = 0;
+  int32_t m_resetCbKey = 0;
+  int32_t m_reverseDirectionCbKey = 0;
+  int32_t m_samplesCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h
new file mode 100644
index 0000000..4b06bee
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderJoystick : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderJoystick();
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_dsNewDataCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h
new file mode 100644
index 0000000..19be17a
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderPWM : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderPWM();
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_speedCbKey = 0;
+  int32_t m_positionCbKey = 0;
+  int32_t m_rawCbKey = 0;
+  int32_t m_periodScaleCbKey = 0;
+  int32_t m_zeroLatchCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h
new file mode 100644
index 0000000..aa3f84f
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderRelay : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderRelay();
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initFwdCbKey = 0;
+  int32_t m_initRevCbKey = 0;
+  int32_t m_fwdCbKey = 0;
+  int32_t m_revCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h
new file mode 100644
index 0000000..9ffc2ed
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderRoboRIO : public HALSimWSHalProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalProvider::HALSimWSHalProvider;
+  ~HALSimWSProviderRoboRIO();
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_fpgaCbKey = 0;
+  int32_t m_vinVoltageCbKey = 0;
+  int32_t m_vinCurrentCbKey = 0;
+  int32_t m_6vVoltageCbKey = 0;
+  int32_t m_6vCurrentCbKey = 0;
+  int32_t m_6vActiveCbKey = 0;
+  int32_t m_6vFaultsCbKey = 0;
+  int32_t m_5vVoltageCbKey = 0;
+  int32_t m_5vCurrentCbKey = 0;
+  int32_t m_5vActiveCbKey = 0;
+  int32_t m_5vFaultsCbKey = 0;
+  int32_t m_3v3VoltageCbKey = 0;
+  int32_t m_3v3CurrentCbKey = 0;
+  int32_t m_3v3ActiveCbKey = 0;
+  int32_t m_3v3FaultsCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h
new file mode 100644
index 0000000..63e5d22
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
+#include <wpi/StringMap.h>
+#include <wpi/uv/AsyncFunction.h>
+
+#include "WSBaseProvider.h"
+#include "WSProviderContainer.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderSimDevice;
+class HALSimWSProviderSimDevices;
+
+struct SimDeviceValueData {
+  HALSimWSProviderSimDevice* device;
+  HAL_SimValueHandle handle;
+  std::string key;
+  HAL_Type valueType;
+};
+
+class HALSimWSProviderSimDevice : public HALSimWSBaseProvider {
+ public:
+  HALSimWSProviderSimDevice(HAL_SimDeviceHandle handle, const std::string& key,
+                            const std::string& deviceId)
+      : HALSimWSBaseProvider(key, "SimDevices"), m_handle(handle) {
+    m_deviceId = deviceId;
+  }
+
+  ~HALSimWSProviderSimDevice();
+
+  void OnNetworkConnected(
+      std::shared_ptr<HALSimBaseWebSocketConnection> ws) override;
+
+  void OnNetworkDisconnected() override;
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+  void ProcessHalCallback(const wpi::json& payload);
+
+ private:
+  static void OnValueCreatedStatic(const char* name, void* param,
+                                   HAL_SimValueHandle handle, HAL_Bool readonly,
+                                   const struct HAL_Value* value) {
+    (reinterpret_cast<HALSimWSProviderSimDevice*>(param))
+        ->OnValueCreated(name, handle, readonly, value);
+  }
+  void OnValueCreated(const char* name, HAL_SimValueHandle handle,
+                      HAL_Bool readonly, const struct HAL_Value* value);
+
+  static void OnValueChangedStatic(const char* name, void* param,
+                                   HAL_SimValueHandle handle, HAL_Bool readonly,
+                                   const struct HAL_Value* value) {
+    auto valueData = (reinterpret_cast<SimDeviceValueData*>(param));
+    valueData->device->OnValueChanged(valueData, value);
+  }
+  void OnValueChanged(SimDeviceValueData* valueData,
+                      const struct HAL_Value* value);
+
+  void CancelCallbacks();
+
+  wpi::StringMap<std::unique_ptr<SimDeviceValueData>> m_valueHandles;
+  std::shared_mutex m_vhLock;
+
+  HAL_SimDeviceHandle m_handle;
+
+  std::shared_ptr<HALSimWSProviderSimDevices> m_simDevicesBase;
+
+  int32_t m_simValueCreatedCbKey = 0;
+  wpi::StringMap<int32_t> m_simValueChangedCbKeys;
+};
+
+class HALSimWSProviderSimDevices {
+ public:
+  using LoopFn = std::function<void(void)>;
+  using UvExecFn = wpi::uv::AsyncFunction<void(LoopFn)>;
+
+  explicit HALSimWSProviderSimDevices(ProviderContainer& providers)
+      : m_providers(providers) {}
+  ~HALSimWSProviderSimDevices();
+
+  void Initialize(wpi::uv::Loop& loop);
+
+  void OnNetworkConnected(std::shared_ptr<HALSimBaseWebSocketConnection> hws);
+  void OnNetworkDisconnected();
+
+  std::shared_ptr<HALSimBaseWebSocketConnection> GetWSConnection() {
+    return m_ws;
+  }
+
+ private:
+  static void DeviceCreatedCallbackStatic(const char* name, void* param,
+                                          HAL_SimDeviceHandle handle) {
+    (reinterpret_cast<HALSimWSProviderSimDevices*>(param))
+        ->DeviceCreatedCallback(name, handle);
+  }
+  void DeviceCreatedCallback(const char* name, HAL_SimDeviceHandle handle);
+
+  static void DeviceFreedCallbackStatic(const char* name, void* param,
+                                        HAL_SimDeviceHandle handle) {
+    (reinterpret_cast<HALSimWSProviderSimDevices*>(param))
+        ->DeviceFreedCallback(name, handle);
+  }
+  void DeviceFreedCallback(const char* name, HAL_SimDeviceHandle handle);
+
+  void CancelCallbacks();
+
+  ProviderContainer& m_providers;
+
+  std::shared_ptr<HALSimBaseWebSocketConnection> m_ws;
+  std::shared_ptr<UvExecFn> m_exec;
+
+  int32_t m_deviceCreatedCbKey = 0;
+  int32_t m_deviceFreedCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h

diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h
new file mode 100644
index 0000000..49b7000
--- /dev/null
+++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+class HALSimWSProviderDigitalPWM : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderDigitalPWM();
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_dutyCycleCbKey = 0;
+  int32_t m_pinCbKey = 0;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_server/CMakeLists.txt

diff --git a/simulation/halsim_ws_server/CMakeLists.txt b/simulation/halsim_ws_server/CMakeLists.txt
new file mode 100644
index 0000000..d1c7983
--- /dev/null
+++ b/simulation/halsim_ws_server/CMakeLists.txt
@@ -0,0 +1,16 @@
+project(halsim_ws_server)
+
+include(CompileWarnings)
+
+file(GLOB halsim_ws_server_src src/main/native/cpp/*.cpp)
+
+add_library(halsim_ws_server MODULE ${halsim_ws_server_src})
+wpilib_target_warnings(halsim_ws_server)
+set_target_properties(halsim_ws_server PROPERTIES DEBUG_POSTFIX "d")
+target_link_libraries(halsim_ws_server PUBLIC hal halsim_ws_core)
+
+target_include_directories(halsim_ws_server PRIVATE src/main/native/include)
+
+set_property(TARGET halsim_ws_server PROPERTY FOLDER "libraries")
+
+install(TARGETS halsim_ws_server EXPORT halsim_ws_server DESTINATION "${main_lib_dest}")

simulation/halsim_ws_server/build.gradle

diff --git a/simulation/halsim_ws_server/build.gradle b/simulation/halsim_ws_server/build.gradle
new file mode 100644
index 0000000..6c6be48
--- /dev/null
+++ b/simulation/halsim_ws_server/build.gradle
@@ -0,0 +1,72 @@
+
+description = "WebSocket Server Extension"
+
+ext {
+    includeWpiutil = true
+    pluginName = 'halsim_ws_server'
+}
+
+apply plugin: 'google-test-test-suite'
+
+
+ext {
+    staticGtestConfigs = [:]
+}
+
+staticGtestConfigs["${pluginName}Test"] = []
+apply from: "${rootDir}/shared/googletest.gradle"
+
+apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
+
+model {
+    testSuites {
+        def comps = $.components
+        if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
+            "${pluginName}Test"(GoogleTestTestSuiteSpec) {
+                for(NativeComponentSpec c : comps) {
+                    if (c.name == pluginName) {
+                        testing c
+                        break
+                    }
+                }
+                sources {
+                    cpp {
+                        source {
+                            srcDirs 'src/test/native/cpp'
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/test/native/include', 'src/main/native/cpp'
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    binaries {
+        all {
+            if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                it.buildable = false
+                return
+            }
+
+            lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static"
+        }
+
+        withType(GoogleTestTestSuiteBinarySpec) {
+            project(':hal').addHalDependency(it, 'shared')
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib library: pluginName, linkage: 'shared'
+            if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+            }
+        }
+    }
+
+}
+
+tasks.withType(RunTestExecutable) {
+    args "--gtest_output=xml:test_detail.xml"
+    outputs.dir outputDir
+}

simulation/halsim_ws_server/src/dev/native/cpp/main.cpp

diff --git a/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp b/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..c98c5e4
--- /dev/null
+++ b/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+#include <thread>
+
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
+#include <hal/Main.h>
+#include <wpi/Format.h>
+#include <wpi/raw_ostream.h>
+
+extern "C" int HALSIM_InitExtension(void);
+
+int main() {
+  HAL_Initialize(500, 0);
+  HALSIM_InitExtension();
+
+  // HAL_ObserveUserProgramStarting();
+
+  HAL_RunMain();
+
+  int cycleCount = 0;
+  while (cycleCount < 1000) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    cycleCount++;
+    std::cout << "Count: " << cycleCount << std::endl;
+  }
+
+  std::cout << "DONE" << std::endl;
+  HAL_ExitMain();
+}

simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp

diff --git a/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp b/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp
new file mode 100644
index 0000000..73ebaca
--- /dev/null
+++ b/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp
@@ -0,0 +1,221 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimHttpConnection.h"
+
+#include <uv.h>
+
+#include <wpi/FileSystem.h>
+#include <wpi/MimeTypes.h>
+#include <wpi/Path.h>
+#include <wpi/SmallVector.h>
+#include <wpi/UrlParser.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/raw_uv_ostream.h>
+#include <wpi/uv/Request.h>
+
+namespace uv = wpi::uv;
+
+using namespace wpilibws;
+
+bool HALSimHttpConnection::IsValidWsUpgrade(wpi::StringRef protocol) {
+  if (m_request.GetUrl() != m_server->GetServerUri()) {
+    MySendError(404, "invalid websocket address");
+    return false;
+  }
+
+  return true;
+}
+
+void HALSimHttpConnection::ProcessWsUpgrade() {
+  m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) {
+    conn.disconnect();  // one-shot
+
+    if (!m_server->RegisterWebsocket(shared_from_this())) {
+      Log(409);
+      m_websocket->Fail(409, "Only a single simulation websocket is allowed");
+      return;
+    }
+
+    Log(200);
+    m_isWsConnected = true;
+    wpi::errs() << "HALWebSim: websocket connected\n";
+  });
+
+  // parse incoming JSON, dispatch to parent
+  m_websocket->text.connect([this](wpi::StringRef msg, bool) {
+    if (!m_isWsConnected) {
+      return;
+    }
+
+    wpi::json j;
+    try {
+      j = wpi::json::parse(msg);
+    } catch (const wpi::json::parse_error& e) {
+      std::string err("JSON parse failed: ");
+      err += e.what();
+      m_websocket->Fail(400, err);
+      return;
+    }
+    m_server->OnNetValueChanged(j);
+  });
+
+  m_websocket->closed.connect([this](uint16_t, wpi::StringRef) {
+    // unset the global, allow another websocket to connect
+    if (m_isWsConnected) {
+      wpi::errs() << "HALWebSim: websocket disconnected\n";
+      m_isWsConnected = false;
+
+      m_server->CloseWebsocket(shared_from_this());
+    }
+  });
+}
+
+void HALSimHttpConnection::OnSimValueChanged(const wpi::json& msg) {
+  // render json to buffers
+  wpi::SmallVector<uv::Buffer, 4> sendBufs;
+  wpi::raw_uv_ostream os{sendBufs, [this]() -> uv::Buffer {
+                           std::lock_guard lock(m_buffers_mutex);
+                           return m_buffers.Allocate();
+                         }};
+  os << msg;
+
+  // call the websocket send function on the uv loop
+  m_server->GetExec().Send([self = shared_from_this(), sendBufs] {
+    self->m_websocket->SendText(sendBufs,
+                                [self](auto bufs, wpi::uv::Error err) {
+                                  {
+                                    std::lock_guard lock(self->m_buffers_mutex);
+                                    self->m_buffers.Release(bufs);
+                                  }
+
+                                  if (err) {
+                                    wpi::errs() << err.str() << "\n";
+                                    wpi::errs().flush();
+                                  }
+                                });
+  });
+}
+
+void HALSimHttpConnection::SendFileResponse(int code,
+                                            const wpi::Twine& codeText,
+                                            const wpi::Twine& contentType,
+                                            const wpi::Twine& filename,
+                                            const wpi::Twine& extraHeader) {
+  // open file
+  int infd;
+  if (wpi::sys::fs::openFileForRead(filename, infd)) {
+    MySendError(404, "error opening file");
+    return;
+  }
+
+  // get status (to get file size)
+  wpi::sys::fs::file_status status;
+  if (wpi::sys::fs::status(infd, status)) {
+    MySendError(404, "error getting file size");
+    wpi::sys::fs::file_t file = uv_get_osfhandle(infd);
+    wpi::sys::fs::closeFile(file);
+    return;
+  }
+
+  uv_os_fd_t outfd;
+  int err = uv_fileno(m_stream.GetRawHandle(), &outfd);
+  if (err < 0) {
+    m_stream.GetLoopRef().ReportError(err);
+    MySendError(404, "error getting fd");
+    wpi::sys::fs::file_t file = uv_get_osfhandle(infd);
+    wpi::sys::fs::closeFile(file);
+    return;
+  }
+
+  wpi::SmallVector<uv::Buffer, 4> toSend;
+  wpi::raw_uv_ostream os{toSend, 4096};
+  BuildHeader(os, code, codeText, contentType, status.getSize(), extraHeader);
+  SendData(os.bufs(), false);
+
+  Log(code);
+
+  // Read the file byte by byte
+  wpi::SmallVector<uv::Buffer, 4> bodyData;
+  wpi::raw_uv_ostream bodyOs{bodyData, 4096};
+
+  wpi::raw_fd_istream is{infd, true};
+  std::string fileBuf;
+  size_t oldSize = 0;
+
+  while (fileBuf.size() < status.getSize()) {
+    oldSize = fileBuf.size();
+    fileBuf.resize(oldSize + 1);
+    is.read(&(*fileBuf.begin()) + oldSize, 1);
+  }
+
+  bodyOs << fileBuf;
+
+  SendData(bodyOs.bufs(), false);
+  if (!m_keepAlive) {
+    m_stream.Close();
+  }
+}
+
+void HALSimHttpConnection::ProcessRequest() {
+  wpi::UrlParser url{m_request.GetUrl(),
+                     m_request.GetMethod() == wpi::HTTP_CONNECT};
+  if (!url.IsValid()) {
+    // failed to parse URL
+    MySendError(400, "Invalid URL");
+    return;
+  }
+
+  wpi::StringRef path;
+  if (url.HasPath()) path = url.GetPath();
+
+  if (m_request.GetMethod() == wpi::HTTP_GET && path.startswith("/") &&
+      !path.contains("..")) {
+    // convert to fs native representation
+    wpi::SmallVector<char, 32> nativePath;
+    wpi::sys::path::native(path, nativePath);
+
+    if (path.startswith("/user/")) {
+      std::string prefix = (wpi::sys::path::get_separator() + "user" +
+                            wpi::sys::path::get_separator())
+                               .str();
+      wpi::sys::path::replace_path_prefix(nativePath, prefix,
+                                          m_server->GetWebrootUser());
+    } else {
+      wpi::sys::path::replace_path_prefix(nativePath,
+                                          wpi::sys::path::get_separator(),
+                                          m_server->GetWebrootSys());
+    }
+
+    if (wpi::sys::fs::is_directory(nativePath)) {
+      wpi::sys::path::append(nativePath, "index.html");
+    }
+
+    if (!wpi::sys::fs::exists(nativePath) ||
+        wpi::sys::fs::is_directory(nativePath)) {
+      MySendError(404, "Resource '" + path + "' not found");
+    } else {
+      auto contentType = wpi::MimeTypeFromPath(wpi::Twine(nativePath).str());
+      SendFileResponse(200, "OK", contentType, nativePath);
+    }
+  } else {
+    MySendError(404, "Resource not found");
+  }
+}
+
+void HALSimHttpConnection::MySendError(int code, const wpi::Twine& message) {
+  Log(code);
+  SendError(code, message);
+}
+
+void HALSimHttpConnection::Log(int code) {
+  auto method = wpi::http_method_str(m_request.GetMethod());
+  wpi::errs() << method << " " << m_request.GetUrl() << " HTTP/"
+              << m_request.GetMajor() << "." << m_request.GetMinor() << " "
+              << code << "\n";
+}

simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp

diff --git a/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp b/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp
new file mode 100644
index 0000000..2bc846b
--- /dev/null
+++ b/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimWSServer.h"
+
+#include <WSProviderContainer.h>
+#include <WSProvider_Analog.h>
+#include <WSProvider_DIO.h>
+#include <WSProvider_DriverStation.h>
+#include <WSProvider_Encoder.h>
+#include <WSProvider_Joystick.h>
+#include <WSProvider_PWM.h>
+#include <WSProvider_Relay.h>
+#include <WSProvider_RoboRIO.h>
+#include <WSProvider_SimDevice.h>
+#include <WSProvider_dPWM.h>
+
+using namespace wpilibws;
+
+bool HALSimWSServer::Initialize() {
+  bool result = true;
+  runner.ExecSync([&](wpi::uv::Loop& loop) {
+    simWeb = std::make_shared<HALSimWeb>(loop, providers, simDevices);
+
+    if (!simWeb->Initialize()) {
+      result = false;
+      return;
+    }
+
+    WSRegisterFunc registerFunc = [&](auto key, auto provider) {
+      providers.Add(key, provider);
+    };
+
+    HALSimWSProviderAnalogIn::Initialize(registerFunc);
+    HALSimWSProviderAnalogOut::Initialize(registerFunc);
+    HALSimWSProviderDIO::Initialize(registerFunc);
+    HALSimWSProviderDigitalPWM::Initialize(registerFunc);
+    HALSimWSProviderDriverStation::Initialize(registerFunc);
+    HALSimWSProviderEncoder::Initialize(registerFunc);
+    HALSimWSProviderJoystick::Initialize(registerFunc);
+    HALSimWSProviderPWM::Initialize(registerFunc);
+    HALSimWSProviderRelay::Initialize(registerFunc);
+    HALSimWSProviderRoboRIO::Initialize(registerFunc);
+
+    simDevices.Initialize(loop);
+
+    simWeb->Start();
+  });
+
+  return result;
+}

simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp

diff --git a/simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp b/simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp
new file mode 100644
index 0000000..fe5393d
--- /dev/null
+++ b/simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp
@@ -0,0 +1,170 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimWeb.h"
+
+#include <wpi/FileSystem.h>
+#include <wpi/Path.h>
+#include <wpi/SmallString.h>
+#include <wpi/Twine.h>
+#include <wpi/UrlParser.h>
+#include <wpi/WebSocketServer.h>
+#include <wpi/raw_uv_ostream.h>
+#include <wpi/uv/Loop.h>
+#include <wpi/uv/Tcp.h>
+
+#include "HALSimHttpConnection.h"
+
+namespace uv = wpi::uv;
+
+using namespace wpilibws;
+
+HALSimWeb::HALSimWeb(wpi::uv::Loop& loop, ProviderContainer& providers,
+                     HALSimWSProviderSimDevices& simDevicesProvider)
+    : m_loop(loop),
+      m_providers(providers),
+      m_simDevicesProvider(simDevicesProvider) {
+  m_loop.error.connect([](uv::Error err) {
+    wpi::errs() << "HALSim WS Server libuv ERROR: " << err.str() << '\n';
+  });
+
+  m_server = uv::Tcp::Create(m_loop);
+  m_exec = UvExecFunc::Create(m_loop);
+  if (m_exec) {
+    m_exec->wakeup.connect([](auto func) { func(); });
+  }
+}
+
+bool HALSimWeb::Initialize() {
+  if (!m_server || !m_exec) {
+    return false;
+  }
+
+  // determine where to get static content from
+  // wpi::SmallVector<char, 64> tmp;
+  wpi::SmallString<64> tmp;
+
+  const char* webroot_sys = std::getenv("HALSIMWS_SYSROOT");
+  if (webroot_sys != NULL) {
+    wpi::StringRef tstr(webroot_sys);
+    tmp.append(tstr);
+  } else {
+    wpi::sys::fs::current_path(tmp);
+    wpi::sys::path::append(tmp, "sim");
+  }
+  wpi::sys::fs::make_absolute(tmp);
+  m_webroot_sys = wpi::Twine(tmp).str();
+
+  tmp.clear();
+  const char* webroot_user = std::getenv("HALSIMWS_USERROOT");
+  if (webroot_user != NULL) {
+    wpi::StringRef tstr(webroot_user);
+    tmp.append(tstr);
+  } else {
+    wpi::sys::fs::current_path(tmp);
+    wpi::sys::path::append(tmp, "sim", "user");
+  }
+  wpi::sys::fs::make_absolute(tmp);
+  m_webroot_user = wpi::Twine(tmp).str();
+
+  const char* uri = std::getenv("HALSIMWS_URI");
+  if (uri != NULL) {
+    m_uri = uri;
+  } else {
+    m_uri = "/wpilibws";
+  }
+
+  const char* port = std::getenv("HALSIMWS_PORT");
+  if (port != NULL) {
+    try {
+      m_port = std::stoi(port);
+    } catch (const std::invalid_argument& err) {
+      wpi::errs() << "Error decoding HALSIMWS_PORT (" << err.what() << ")\n";
+      return false;
+    }
+  } else {
+    m_port = 8080;
+  }
+
+  return true;
+}
+
+void HALSimWeb::Start() {
+  m_server->Bind("", m_port);
+
+  // when we get a connection, accept it and start reading
+  m_server->connection.connect([this, srv = m_server.get()] {
+    auto tcp = srv->Accept();
+    if (!tcp) return;
+
+    tcp->SetNoDelay(true);
+
+    auto conn = std::make_shared<HALSimHttpConnection>(shared_from_this(), tcp);
+    tcp->SetData(conn);
+  });
+
+  // start listening for incoming connections
+  m_server->Listen();
+  wpi::outs() << "Listening at http://localhost:" << m_port << "\n";
+  wpi::outs() << "WebSocket URI: " << m_uri << "\n";
+}
+
+bool HALSimWeb::RegisterWebsocket(
+    std::shared_ptr<HALSimBaseWebSocketConnection> hws) {
+  if (m_hws.lock()) {
+    return false;
+  }
+
+  m_hws = hws;
+
+  m_simDevicesProvider.OnNetworkConnected(hws);
+
+  // notify all providers that they should use this new websocket instead
+  m_providers.ForEach([hws](std::shared_ptr<HALSimWSBaseProvider> provider) {
+    provider->OnNetworkConnected(hws);
+  });
+
+  return true;
+}
+
+void HALSimWeb::CloseWebsocket(
+    std::shared_ptr<HALSimBaseWebSocketConnection> hws) {
+  // Inform the providers that they need to cancel callbacks
+  m_simDevicesProvider.OnNetworkDisconnected();
+
+  m_providers.ForEach([](std::shared_ptr<HALSimWSBaseProvider> provider) {
+    provider->OnNetworkDisconnected();
+  });
+
+  if (hws == m_hws.lock()) {
+    m_hws.reset();
+  }
+}
+
+void HALSimWeb::OnNetValueChanged(const wpi::json& msg) {
+  // Look for "type" and "device" fields so that we can
+  // generate the key
+
+  try {
+    auto& type = msg.at("type").get_ref<const std::string&>();
+    auto& device = msg.at("device").get_ref<const std::string&>();
+
+    wpi::SmallString<64> key;
+    key.append(type);
+    if (!device.empty()) {
+      key.append("/");
+      key.append(device);
+    }
+
+    auto provider = m_providers.Get(key.str());
+    if (provider) {
+      provider->OnNetValueChanged(msg.at("data"));
+    }
+  } catch (wpi::json::exception& e) {
+    wpi::errs() << "Error with incoming message: " << e.what() << "\n";
+  }
+}

simulation/halsim_ws_server/src/main/native/cpp/main.cpp

diff --git a/simulation/halsim_ws_server/src/main/native/cpp/main.cpp b/simulation/halsim_ws_server/src/main/native/cpp/main.cpp
new file mode 100644
index 0000000..025a902
--- /dev/null
+++ b/simulation/halsim_ws_server/src/main/native/cpp/main.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+
+#include <hal/Extensions.h>
+#include <wpi/raw_ostream.h>
+
+#include "HALSimWSServer.h"
+
+using namespace std::placeholders;
+using namespace wpilibws;
+
+static std::unique_ptr<HALSimWSServer> gServer;
+
+extern "C" {
+#if defined(WIN32) || defined(_WIN32)
+__declspec(dllexport)
+#endif
+    int HALSIM_InitExtension(void) {
+  wpi::outs() << "Websocket WS Server Initializing.\n";
+
+  HAL_OnShutdown(nullptr, [](void*) { gServer.reset(); });
+
+  gServer = std::make_unique<HALSimWSServer>();
+  if (!gServer->Initialize()) {
+    return -1;
+  }
+
+  wpi::outs() << "Websocket WS Server Initialized!\n";
+  return 0;
+}
+}  // extern "C"

simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h

diff --git a/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h b/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h
new file mode 100644
index 0000000..89e6c83
--- /dev/null
+++ b/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cinttypes>
+#include <memory>
+#include <utility>
+
+#include <HALSimBaseWebSocketConnection.h>
+#include <wpi/HttpWebSocketServerConnection.h>
+#include <wpi/mutex.h>
+#include <wpi/uv/AsyncFunction.h>
+#include <wpi/uv/Buffer.h>
+
+#include "HALSimWeb.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace wpilibws {
+
+class HALSimHttpConnection
+    : public wpi::HttpWebSocketServerConnection<HALSimHttpConnection>,
+      public HALSimBaseWebSocketConnection {
+ public:
+  HALSimHttpConnection(std::shared_ptr<HALSimWeb> server,
+                       std::shared_ptr<wpi::uv::Stream> stream)
+      : wpi::HttpWebSocketServerConnection<HALSimHttpConnection>(stream, {}),
+        m_server(std::move(server)),
+        m_buffers(128) {}
+
+ public:
+  // callable from any thread
+  void OnSimValueChanged(const wpi::json& msg) override;
+
+ protected:
+  void ProcessRequest() override;
+  bool IsValidWsUpgrade(wpi::StringRef protocol) override;
+  void ProcessWsUpgrade() override;
+  void SendFileResponse(int code, const wpi::Twine& codeText,
+                        const wpi::Twine& contentType,
+                        const wpi::Twine& filename,
+                        const wpi::Twine& extraHeader = wpi::Twine{});
+
+  void MySendError(int code, const wpi::Twine& message);
+  void Log(int code);
+
+ private:
+  std::shared_ptr<HALSimWeb> m_server;
+
+  // is the websocket connected?
+  bool m_isWsConnected = false;
+
+  // these are only valid if the websocket is connected
+  wpi::uv::SimpleBufferPool<4> m_buffers;
+  std::mutex m_buffers_mutex;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h

diff --git a/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h b/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h
new file mode 100644
index 0000000..d75341b
--- /dev/null
+++ b/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpi/EventLoopRunner.h>
+
+#include "HALSimWeb.h"
+
+namespace wpilibws {
+
+class HALSimWSServer {
+ public:
+  HALSimWSServer() = default;
+  HALSimWSServer(const HALSimWSServer&) = delete;
+  HALSimWSServer& operator=(const HALSimWSServer&) = delete;
+
+  bool Initialize();
+
+  ProviderContainer providers;
+  HALSimWSProviderSimDevices simDevices{providers};
+  wpi::EventLoopRunner runner;
+  std::shared_ptr<HALSimWeb> simWeb;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h

diff --git a/simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h b/simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h
new file mode 100644
index 0000000..f71cbdf
--- /dev/null
+++ b/simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <WSBaseProvider.h>
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpi/StringRef.h>
+#include <wpi/uv/Async.h>
+#include <wpi/uv/Loop.h>
+#include <wpi/uv/Tcp.h>
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace wpilibws {
+
+class HALSimWeb : public std::enable_shared_from_this<HALSimWeb> {
+ public:
+  using LoopFunc = std::function<void(void)>;
+  using UvExecFunc = wpi::uv::Async<LoopFunc>;
+
+  HALSimWeb(wpi::uv::Loop& loop, ProviderContainer& providers,
+            HALSimWSProviderSimDevices& simDevicesProvider);
+
+  HALSimWeb(const HALSimWeb&) = delete;
+  HALSimWeb& operator=(const HALSimWeb&) = delete;
+
+  bool Initialize();
+  void Start();
+
+  bool RegisterWebsocket(std::shared_ptr<HALSimBaseWebSocketConnection> hws);
+  void CloseWebsocket(std::shared_ptr<HALSimBaseWebSocketConnection> hws);
+
+  // network -> sim
+  void OnNetValueChanged(const wpi::json& msg);
+
+  wpi::StringRef GetWebrootSys() const { return m_webroot_sys; }
+  wpi::StringRef GetWebrootUser() const { return m_webroot_user; }
+  wpi::StringRef GetServerUri() const { return m_uri; }
+  int GetServerPort() const { return m_port; }
+  wpi::uv::Loop& GetLoop() { return m_loop; }
+
+  UvExecFunc& GetExec() { return *m_exec; }
+
+ private:
+  // connected http connection that contains active websocket
+  std::weak_ptr<HALSimBaseWebSocketConnection> m_hws;
+
+  wpi::uv::Loop& m_loop;
+  std::shared_ptr<wpi::uv::Tcp> m_server;
+  std::shared_ptr<UvExecFunc> m_exec;
+
+  // list of providers
+  ProviderContainer& m_providers;
+  HALSimWSProviderSimDevices& m_simDevicesProvider;
+
+  // Absolute paths of folders to retrieve data from
+  // -> /
+  std::string m_webroot_sys;
+  // -> /user
+  std::string m_webroot_user;
+
+  std::string m_uri;
+  int m_port;
+};
+
+}  // namespace wpilibws

simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp

diff --git a/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp b/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp
new file mode 100644
index 0000000..6cdac5a
--- /dev/null
+++ b/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp
@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WebServerClientTest.h"
+
+#include <sstream>
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/raw_uv_ostream.h>
+#include <wpi/uv/util.h>
+
+static constexpr int kTcpConnectAttemptTimeout = 1000;
+
+namespace uv = wpi::uv;
+
+namespace wpilibws {
+
+// Create Web Socket and specify event callbacks
+void WebServerClientTest::InitializeWebSocket(const std::string& host, int port,
+                                              const std::string& uri) {
+  std::stringstream ss;
+  ss << host << ":" << port;
+  wpi::outs() << "Will attempt to connect to: " << ss.str() << uri << "\n";
+  m_websocket =
+      wpi::WebSocket::CreateClient(*m_tcp_client.get(), uri, ss.str());
+
+  // Hook up events
+  m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) {
+    conn.disconnect();
+    m_buffers = std::make_unique<BufferPool>();
+
+    m_exec = UvExecFunc::Create(m_tcp_client->GetLoop(),
+                                [](auto out, LoopFunc func) {
+                                  func();
+                                  out.set_value();
+                                });
+
+    m_ws_connected = true;
+    wpi::errs() << "WebServerClientTest: WebSocket Connected\n";
+  });
+
+  m_websocket->text.connect([this](wpi::StringRef msg, bool) {
+    wpi::json j;
+    try {
+      j = wpi::json::parse(msg);
+    } catch (const wpi::json::parse_error& e) {
+      std::string err("JSON parse failed: ");
+      err += e.what();
+      wpi::errs() << err << "\n";
+      m_websocket->Fail(1003, err);
+      return;
+    }
+    // Save last message received
+    m_json = j;
+  });
+
+  m_websocket->closed.connect([this](uint16_t, wpi::StringRef) {
+    if (m_ws_connected) {
+      wpi::errs() << "WebServerClientTest: Websocket Disconnected\n";
+      m_ws_connected = false;
+    }
+  });
+}
+
+// Create tcp client, specify callbacks, and create timers for loop
+bool WebServerClientTest::Initialize() {
+  m_loop.error.connect(
+      [](uv::Error err) { wpi::errs() << "uv Error: " << err.str() << "\n"; });
+
+  m_tcp_client = uv::Tcp::Create(m_loop);
+  if (!m_tcp_client) {
+    wpi::errs() << "ERROR: Could not create TCP Client\n";
+    return false;
+  }
+
+  // Hook up TCP client events
+  m_tcp_client->error.connect(
+      [this, socket = m_tcp_client.get()](wpi::uv::Error err) {
+        if (m_tcp_connected) {
+          m_tcp_connected = false;
+          m_connect_attempts = 0;
+          m_loop.Stop();
+          return;
+        }
+
+        // If we weren't previously connected, attempt a reconnection
+        m_connect_timer->Start(uv::Timer::Time(kTcpConnectAttemptTimeout));
+      });
+
+  m_tcp_client->closed.connect(
+      []() { wpi::errs() << "TCP connection closed\n"; });
+
+  // Set up the connection timer
+  m_connect_timer = uv::Timer::Create(m_loop);
+  if (!m_connect_timer) {
+    return false;
+  }
+  // Set up the timer to attempt connection
+  m_connect_timer->timeout.connect([this] { AttemptConnect(); });
+  m_connect_timer->Start(uv::Timer::Time(0));
+
+  wpi::outs() << "WebServerClientTest Initialized\n";
+
+  return true;
+}
+
+void WebServerClientTest::AttemptConnect() {
+  m_connect_attempts++;
+  wpi::outs() << "Test Client Connection Attempt " << m_connect_attempts
+              << "\n";
+
+  if (m_connect_attempts >= 5) {
+    wpi::errs() << "Test Client Timeout. Unable to connect\n";
+    m_loop.Stop();
+    return;
+  }
+
+  struct sockaddr_in dest;
+  uv::NameToAddr("localhost", 8080, &dest);
+  m_tcp_client->Connect(dest, [this]() {
+    m_tcp_connected = true;
+    InitializeWebSocket("localhost", 8080, "/wpilibws");
+  });
+}
+
+void WebServerClientTest::SendMessage(const wpi::json& msg) {
+  if (msg.empty()) {
+    wpi::errs() << "Message to send is empty\n";
+    return;
+  }
+
+  wpi::SmallVector<uv::Buffer, 4> sendBufs;
+
+  wpi::raw_uv_ostream os{sendBufs, [this]() -> uv::Buffer {
+                           std::lock_guard lock(m_buffers_mutex);
+                           return m_buffers->Allocate();
+                         }};
+  os << msg;
+
+  // Call the websocket send function on the uv loop
+  m_exec->Call([this, sendBufs]() mutable {
+    m_websocket->SendText(sendBufs, [this](auto bufs, wpi::uv::Error err) {
+      {
+        std::lock_guard lock(m_buffers_mutex);
+        m_buffers->Release(bufs);
+      }
+      if (err) {
+        wpi::errs() << err.str() << "\n";
+        wpi::errs().flush();
+      }
+    });
+  });
+}
+
+const wpi::json& WebServerClientTest::GetLastMessage() { return m_json; }
+
+}  // namespace wpilibws

simulation/halsim_ws_server/src/test/native/cpp/main.cpp

diff --git a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..4172f19
--- /dev/null
+++ b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp
@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <thread>
+
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
+#include <hal/Main.h>
+#include <hal/simulation/DIOData.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/uv/Loop.h>
+
+#include "HALSimWSServer.h"
+#include "WebServerClientTest.h"
+#include "gtest/gtest.h"
+
+namespace uv = wpi::uv;
+
+using namespace wpilibws;
+
+static const int POLLING_SPEED = 10;  // 10 ms polling
+
+class WebServerIntegrationTest : public ::testing::Test {
+ public:
+  WebServerIntegrationTest() {
+    // Initialize server
+    m_server.Initialize();
+
+    // Create and initialize client
+    m_server.runner.ExecSync([=](auto& loop) {
+      m_webserverClient = std::make_shared<WebServerClientTest>(loop);
+      m_webserverClient->Initialize();
+    });
+  }
+
+  bool IsConnectedClientWS() { return m_webserverClient->IsConnectedWS(); }
+
+ protected:
+  std::shared_ptr<WebServerClientTest> m_webserverClient;
+  HALSimWSServer m_server;
+};
+
+TEST_F(WebServerIntegrationTest, DigitalOutput) {
+  // Create expected results
+  const bool EXPECTED_VALUE = false;
+  const int PIN = 0;
+  bool done = false;
+
+  // Attach timer to loop for test function
+  m_server.runner.ExecSync([&](auto& loop) {
+    auto timer = wpi::uv::Timer::Create(loop);
+    timer->timeout.connect([&] {
+      if (done) {
+        return;
+      }
+      if (IsConnectedClientWS()) {
+        wpi::outs() << "***** Setting DIO value for pin " << PIN << " to "
+                    << (EXPECTED_VALUE ? "true" : "false") << "\n";
+        HALSIM_SetDIOValue(PIN, EXPECTED_VALUE);
+        done = true;
+      }
+    });
+    timer->Start(uv::Timer::Time(POLLING_SPEED),
+                 uv::Timer::Time(POLLING_SPEED));
+    timer->Unreference();
+  });
+
+  using namespace std::chrono_literals;
+  std::this_thread::sleep_for(1s);
+
+  // Get values from JSON message
+  std::string test_type;
+  std::string test_device;
+  bool test_value = true;  // Default value from HAL initialization
+  try {
+    auto& msg = m_webserverClient->GetLastMessage();
+    test_type = msg.at("type").get_ref<const std::string&>();
+    test_device = msg.at("device").get_ref<const std::string&>();
+    auto& data = msg.at("data");
+    wpi::json::const_iterator it = data.find("<>value");
+    if (it != data.end()) {
+      test_value = it.value();
+    }
+  } catch (wpi::json::exception& e) {
+    wpi::errs() << "Error with incoming message: " << e.what() << "\n";
+  }
+
+  // Compare results
+  EXPECT_EQ("DIO", test_type);
+  EXPECT_EQ(std::to_string(PIN), test_device);
+  EXPECT_EQ(EXPECTED_VALUE, test_value);
+}
+
+TEST_F(WebServerIntegrationTest, DigitalInput) {
+  // Create expected results
+  const bool EXPECTED_VALUE = false;
+  const int PIN = 0;
+  bool done = false;
+
+  // Attach timer to loop for test function
+  m_server.runner.ExecSync([&](auto& loop) {
+    auto timer = wpi::uv::Timer::Create(loop);
+    timer->timeout.connect([&] {
+      if (done) {
+        return;
+      }
+      if (IsConnectedClientWS()) {
+        wpi::json msg = {{"type", "DIO"},
+                         {"device", std::to_string(PIN)},
+                         {"data", {{"<>value", EXPECTED_VALUE}}}};
+        wpi::outs() << "***** Input JSON: " << msg.dump() << "\n";
+        m_webserverClient->SendMessage(msg);
+        done = true;
+      }
+    });
+    timer->Start(uv::Timer::Time(POLLING_SPEED),
+                 uv::Timer::Time(POLLING_SPEED));
+    timer->Unreference();
+  });
+
+  using namespace std::chrono_literals;
+  std::this_thread::sleep_for(1s);
+
+  // Compare results
+  bool test_value = HALSIM_GetDIOValue(PIN);
+  EXPECT_EQ(EXPECTED_VALUE, test_value);
+}
+
+TEST_F(WebServerIntegrationTest, DriverStation) {
+  // Create expected results
+  const bool EXPECTED_VALUE = true;
+  bool done = false;
+
+  // Attach timer to loop for test function
+  m_server.runner.ExecSync([&](auto& loop) {
+    auto timer = wpi::uv::Timer::Create(loop);
+    timer->timeout.connect([&] {
+      if (done) {
+        return;
+      }
+      if (IsConnectedClientWS()) {
+        wpi::json msg = {
+            {"type", "DriverStation"},
+            {"device", ""},
+            {"data", {{">enabled", EXPECTED_VALUE}, {">new_data", true}}}};
+        wpi::outs() << "***** Input JSON: " << msg.dump() << "\n";
+        m_webserverClient->SendMessage(msg);
+        done = true;
+      }
+    });
+    timer->Start(uv::Timer::Time(POLLING_SPEED),
+                 uv::Timer::Time(POLLING_SPEED));
+    timer->Unreference();
+  });
+
+  using namespace std::chrono_literals;
+  std::this_thread::sleep_for(1s);
+
+  // Compare results
+  HAL_ControlWord cw;
+  HAL_GetControlWord(&cw);
+  bool test_value = cw.enabled;
+  EXPECT_EQ(EXPECTED_VALUE, test_value);
+}
+
+int main(int argc, char* argv[]) {
+  ::testing::InitGoogleTest(&argc, argv);
+  HAL_Initialize(500, 0);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h

diff --git a/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h b/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h
new file mode 100644
index 0000000..909c4ba
--- /dev/null
+++ b/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/WebSocket.h>
+#include <wpi/json.h>
+#include <wpi/uv/AsyncFunction.h>
+#include <wpi/uv/Buffer.h>
+#include <wpi/uv/Loop.h>
+#include <wpi/uv/Stream.h>
+#include <wpi/uv/Tcp.h>
+#include <wpi/uv/Timer.h>
+
+namespace wpilibws {
+
+class WebServerClientTest {
+ public:
+  using BufferPool = wpi::uv::SimpleBufferPool<4>;
+  using LoopFunc = std::function<void(void)>;
+  using UvExecFunc = wpi::uv::AsyncFunction<void(LoopFunc)>;
+
+  explicit WebServerClientTest(wpi::uv::Loop& loop) : m_loop(loop) {}
+  WebServerClientTest(const WebServerClientTest&) = delete;
+  WebServerClientTest& operator=(const WebServerClientTest&) = delete;
+
+  bool Initialize();
+  void AttemptConnect();
+
+  void SendMessage(const wpi::json& msg);
+  const wpi::json& GetLastMessage();
+  bool IsConnectedWS() { return m_ws_connected; }
+
+ private:
+  void InitializeWebSocket(const std::string& host, int port,
+                           const std::string& uri);
+
+  bool m_tcp_connected = false;
+  std::shared_ptr<wpi::uv::Timer> m_connect_timer;
+  int m_connect_attempts = 0;
+  wpi::uv::Loop& m_loop;
+  std::shared_ptr<wpi::uv::Tcp> m_tcp_client;
+  wpi::json m_json;
+
+  bool m_ws_connected = false;
+  std::shared_ptr<wpi::WebSocket> m_websocket;
+  std::shared_ptr<UvExecFunc> m_exec;
+  std::unique_ptr<BufferPool> m_buffers;
+  std::mutex m_buffers_mutex;
+};
+
+}  // namespace wpilibws

simulation/lowfi_simulation/build.gradle

diff --git a/simulation/lowfi_simulation/build.gradle b/simulation/lowfi_simulation/build.gradle
deleted file mode 100644
index e6e8143..0000000
--- a/simulation/lowfi_simulation/build.gradle
+++ /dev/null
@@ -1,194 +0,0 @@
-apply plugin: 'cpp'
-apply plugin: 'google-test-test-suite'
-apply plugin: 'visual-studio'
-apply plugin: 'edu.wpi.first.NativeUtils'
-apply plugin: SingleNativeBuild
-apply plugin: ExtraTasks
-
-
-ext {
-    nativeName = 'lowfi_sim'
-}
-
-apply from: "${rootDir}/shared/config.gradle"
-
-if (!project.hasProperty('onlylinuxathena')) {
-    ext.skiplinuxathena = true
-    ext {
-        sharedCvConfigs = [lowfi_simTest: []]
-        staticCvConfigs = [:]
-        useJava = false
-        useCpp = true
-    }
-    apply from: "${rootDir}/shared/opencv.gradle"
-
-    ext {
-        staticGtestConfigs = [:]
-    }
-    staticGtestConfigs["${nativeName}Test"] = []
-    apply from: "${rootDir}/shared/googletest.gradle"
-
-    project(':').libraryBuild.dependsOn build
-
-    nativeUtils.exportsConfigs {
-        lowfi_sim {
-            x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                                '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                                '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                                '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-            x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                                '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                                '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                                '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        }
-    }
-
-    model {
-        components {
-            "${nativeName}Base"(NativeLibrarySpec) {
-                sources {
-                    cpp {
-                        source {
-                            srcDirs = ['src/main/native/cpp']
-                            include '**/*.cpp'
-                        }
-                        exportedHeaders {
-                            srcDirs 'src/main/native/include'
-                        }
-                    }
-                }
-                binaries.all {
-                    if (it instanceof SharedLibraryBinarySpec) {
-                        it.buildable = false
-                        return
-                    }
-                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        it.buildable = false
-                        return
-                    }
-                    project(':hal').addHalDependency(it, 'shared')
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                }
-            }
-            "${nativeName}"(NativeLibrarySpec) {
-                sources {
-                    cpp {
-                        source {
-                            srcDirs "${rootDir}/shared/singlelib"
-                            include '**/*.cpp'
-                        }
-                        exportedHeaders {
-                            srcDirs 'src/main/native/include'
-                        }
-                    }
-                }
-                binaries.all {
-                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        it.buildable = false
-                        return
-                    }
-                    project(':hal').addHalDependency(it, 'shared')
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                }
-            }
-            // By default, a development executable will be generated. This is to help the case of
-            // testing specific functionality of the library.
-            "${nativeName}Dev"(NativeExecutableSpec) {
-                targetBuildTypes 'debug'
-                sources {
-                    cpp {
-                        source {
-                            srcDirs 'src/dev/native/cpp'
-                            include '**/*.cpp'
-                            lib library: "${nativeName}"
-                        }
-                        exportedHeaders {
-                            srcDirs 'src/dev/native/include'
-                        }
-                    }
-                }
-                binaries.all {
-                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        it.buildable = false
-                        return
-                    }
-                    project(':hal').addHalDependency(it, 'shared')
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                    lib library: nativeName, linkage: 'shared'
-                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
-                    }
-                }
-            }
-        }
-        binaries {
-            withType(GoogleTestTestSuiteBinarySpec) {
-                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
-                lib project: ':cscore', library: 'cscore', linkage: 'shared'
-                project(':hal').addHalDependency(it, 'shared')
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
-                lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
-                lib library: nativeName, linkage: 'shared'
-                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
-                }
-            }
-        }
-    }
-
-    apply from: "publish.gradle"
-}
-
-model {
-
-    testSuites {
-        if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
-            "${nativeName}Test"(GoogleTestTestSuiteSpec) {
-                for(NativeComponentSpec c : $.components) {
-                    if (c.name == nativeName) {
-                        testing c
-                        break
-                    }
-                }
-                sources {
-                    cpp {
-                        source {
-                            srcDirs 'src/test/native/cpp'
-                            include '**/*.cpp'
-                        }
-                        exportedHeaders {
-                            srcDirs 'src/test/native/include', 'src/main/native/cpp'
-                        }
-                    }
-                }
-            }
-        }
-    }
-    tasks {
-        def c = $.components
-        project.tasks.create('runCpp', Exec) {
-            def found = false
-            c.each {
-                if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
-                    it.binaries.each {
-                        if (!found) {
-                            def arch = it.targetPlatform.architecture.name
-                            if (arch == 'x86-64' || arch == 'x86') {
-                                dependsOn it.tasks.install
-								commandLine it.tasks.install.runScriptFile.get().asFile.toString()
-
-                                found = true
-                            }
-                        }
-                    }
-                }
-            }
-        }
-    }
-}
-
-tasks.withType(RunTestExecutable) {
-    args "--gtest_output=xml:test_detail.xml"
-    outputs.dir outputDir
-}

simulation/lowfi_simulation/publish.gradle

diff --git a/simulation/lowfi_simulation/publish.gradle b/simulation/lowfi_simulation/publish.gradle
deleted file mode 100644
index 1fa3aaf..0000000
--- a/simulation/lowfi_simulation/publish.gradle
+++ /dev/null
@@ -1,64 +0,0 @@
-apply plugin: 'maven-publish'
-
-def baseArtifactId = nativeName
-def artifactGroupId = 'edu.wpi.first.halsim'
-def zipBaseName = "_GROUP_edu_wpi_first_halsim_ID_${nativeName}_CLS"
-
-def outputsFolder = file("$project.buildDir/outputs")
-
-task cppSourcesZip(type: Zip) {
-    destinationDirectory = outputsFolder
-    archiveBaseName = zipBaseName
-    classifier = "sources"
-
-    from(licenseFile) {
-        into '/'
-    }
-
-    from('src/main/native/cpp') {
-        into '/'
-    }
-}
-
-task cppHeadersZip(type: Zip) {
-    destinationDirectory = outputsFolder
-    archiveBaseName = zipBaseName
-    classifier = "headers"
-
-    from(licenseFile) {
-        into '/'
-    }
-
-    from('src/main/native/include') {
-        into '/'
-    }
-}
-
-build.dependsOn cppSourcesZip
-build.dependsOn cppHeadersZip
-
-addTaskToCopyAllOutputs(cppSourcesZip)
-addTaskToCopyAllOutputs(cppHeadersZip)
-
-
-model {
-    publishing {
-        def lowfiSimTaskList = createComponentZipTasks($.components, [nativeName], zipBaseName, Zip, project, includeStandardZipFormat)
-
-        publications {
-            cpp(MavenPublication) {
-                lowfiSimTaskList.each {
-                    artifact it
-                }
-
-                artifact cppHeadersZip
-                artifact cppSourcesZip
-
-
-                artifactId = baseArtifactId
-                groupId artifactGroupId
-                version wpilibVersioning.version.get()
-            }
-        }
-    }
-}

simulation/lowfi_simulation/src/dev/native/cpp/main.cpp

diff --git a/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp b/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp
deleted file mode 100644
index eb83810..0000000
--- a/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <iostream>
-
-#include <hal/HALBase.h>
-
-int main() {
-  std::cout << "Hello World" << std::endl;
-  std::cout << HAL_GetRuntimeType() << std::endl;
-}

simulation/lowfi_simulation/src/main/native/cpp/lowfisim/MotorEncoderConnector.cpp

diff --git a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/MotorEncoderConnector.cpp b/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/MotorEncoderConnector.cpp
deleted file mode 100644
index 5e530e6..0000000
--- a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/MotorEncoderConnector.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "lowfisim/MotorEncoderConnector.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-MotorEncoderConnector::MotorEncoderConnector(MotorSim& motorController,
-                                             EncoderSim& encoder)
-    : motorSimulator(motorController), encoderSimulator(encoder) {}
-
-void MotorEncoderConnector::Update() {
-  encoderSimulator.SetPosition(motorSimulator.GetPosition());
-  encoderSimulator.SetVelocity(motorSimulator.GetVelocity());
-}
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/cpp/lowfisim/SimulatorComponentBase.cpp

diff --git a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/SimulatorComponentBase.cpp b/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/SimulatorComponentBase.cpp
deleted file mode 100644
index a65de3b..0000000
--- a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/SimulatorComponentBase.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "lowfisim/SimulatorComponentBase.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-const std::string& SimulatorComponentBase::GetDisplayName() const {
-  return m_name;
-}
-
-void SimulatorComponentBase::SetDisplayName(const std::string& displayName) {
-  m_name = displayName;
-}
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/cpp/lowfisim/motormodel/SimpleMotorModel.cpp

diff --git a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/motormodel/SimpleMotorModel.cpp b/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/motormodel/SimpleMotorModel.cpp
deleted file mode 100644
index 226a836..0000000
--- a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/motormodel/SimpleMotorModel.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "lowfisim/motormodel/SimpleMotorModel.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-SimpleMotorModel::SimpleMotorModel(double maxSpeed) : m_maxSpeed(maxSpeed) {}
-
-void SimpleMotorModel::Reset() {
-  m_position = 0;
-  m_velocity = 0;
-}
-
-void SimpleMotorModel::SetVoltage(double voltage) {
-  m_voltagePercentage = voltage / kMaxExpectedVoltage;
-}
-
-void SimpleMotorModel::Update(double elapsedTime) {
-  m_velocity = m_maxSpeed * m_voltagePercentage;
-  m_position += m_velocity * elapsedTime;
-}
-
-double SimpleMotorModel::GetPosition() const { return m_position; }
-
-double SimpleMotorModel::GetVelocity() const { return m_velocity; }
-
-double SimpleMotorModel::GetAcceleration() const { return 0; }
-
-double SimpleMotorModel::GetCurrent() const { return 0; }
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiAnalogGyroSim.cpp

diff --git a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiAnalogGyroSim.cpp b/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiAnalogGyroSim.cpp
deleted file mode 100644
index 63619cc..0000000
--- a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiAnalogGyroSim.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "lowfisim/wpisimulators/WpiAnalogGyroSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-WpiAnalogGyroSim::WpiAnalogGyroSim(int index) : m_gyroSimulator(index) {}
-
-bool WpiAnalogGyroSim::IsWrapperInitialized() const {
-  return m_gyroSimulator.GetInitialized();
-}
-
-void WpiAnalogGyroSim::SetAngle(double angle) {
-  m_gyroSimulator.SetAngle(angle);
-}
-
-double WpiAnalogGyroSim::GetAngle() { return m_gyroSimulator.GetAngle(); }
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiEncoderSim.cpp

diff --git a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiEncoderSim.cpp b/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiEncoderSim.cpp
deleted file mode 100644
index 7425efe..0000000
--- a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiEncoderSim.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "lowfisim/wpisimulators/WpiEncoderSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-WpiEncoderSim::WpiEncoderSim(int index) : m_encoderSimulator(index) {}
-
-bool WpiEncoderSim::IsWrapperInitialized() const {
-  return m_encoderSimulator.GetInitialized();
-}
-
-void WpiEncoderSim::SetPosition(double position) {
-  m_encoderSimulator.SetCount(
-      static_cast<int>(position / m_encoderSimulator.GetDistancePerPulse()));
-}
-
-void WpiEncoderSim::SetVelocity(double velocity) {
-  m_encoderSimulator.SetPeriod(1.0 / velocity);
-}
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiMotorSim.cpp

diff --git a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiMotorSim.cpp b/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiMotorSim.cpp
deleted file mode 100644
index 0b08cf2..0000000
--- a/simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiMotorSim.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "lowfisim/wpisimulators/WpiMotorSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-WpiMotorSim::WpiMotorSim(int index, MotorModel& motorModelSimulator)
-    : m_motorModelSimulation(motorModelSimulator), m_pwmSimulator(index) {}
-
-void WpiMotorSim::Update(double elapsedTime) {
-  m_motorModelSimulation.SetVoltage(m_pwmSimulator.GetSpeed() *
-                                    kDefaultVoltage);
-  m_motorModelSimulation.Update(elapsedTime);
-}
-
-bool WpiMotorSim::IsWrapperInitialized() const {
-  return m_pwmSimulator.GetInitialized();
-}
-
-double WpiMotorSim::GetPosition() const {
-  return m_motorModelSimulation.GetPosition();
-}
-
-double WpiMotorSim::GetVelocity() const {
-  return m_motorModelSimulation.GetVelocity();
-}
-
-double WpiMotorSim::GetAcceleration() const {
-  return m_motorModelSimulation.GetAcceleration();
-}
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/AccelerometerSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/AccelerometerSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/AccelerometerSim.h
deleted file mode 100644
index 041f257..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/AccelerometerSim.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/SimulatorComponentBase.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class AccelerometerSim : public SimulatorComponentBase {
- public:
-  virtual double GetAcceleration() = 0;
-  virtual void SetAcceleration(double acceleration) = 0;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/EncoderSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/EncoderSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/EncoderSim.h
deleted file mode 100644
index 2e14755..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/EncoderSim.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/SimulatorComponentBase.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class EncoderSim : public SimulatorComponentBase {
- public:
-  virtual void SetPosition(double position) = 0;
-  virtual void SetVelocity(double velocity) = 0;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/GyroSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/GyroSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/GyroSim.h
deleted file mode 100644
index bc80598..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/GyroSim.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/SimulatorComponentBase.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class GyroSim : public SimulatorComponentBase {
- public:
-  virtual void SetAngle(double angle) = 0;
-  virtual double GetAngle() = 0;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorEncoderConnector.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorEncoderConnector.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorEncoderConnector.h
deleted file mode 100644
index 0cf4873..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorEncoderConnector.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "EncoderSim.h"
-#include "MotorSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class MotorEncoderConnector {
- public:
-  MotorEncoderConnector(MotorSim& motorController, EncoderSim& encoder);
-
-  void Update();
-
- private:
-  MotorSim& motorSimulator;
-  EncoderSim& encoderSimulator;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorSim.h
deleted file mode 100644
index 77532a6..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/MotorSim.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/SimulatorComponentBase.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class MotorSim : public SimulatorComponentBase {
- public:
-  virtual double GetPosition() const = 0;
-  virtual double GetVelocity() const = 0;
-  virtual double GetAcceleration() const = 0;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleAccelerometerSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleAccelerometerSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleAccelerometerSim.h
deleted file mode 100644
index 482ac11..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleAccelerometerSim.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-
-#include "lowfisim/AccelerometerSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class SimpleAccelerometerSim : public AccelerometerSim {
- public:
-  SimpleAccelerometerSim(const std::function<bool(void)>& initializedFunction,
-                         const std::function<void(double)>& setterFunction,
-                         const std::function<double(void)>& getterFunction)
-      : m_isInitializedFunction(initializedFunction),
-        m_setAccelerationFunction(setterFunction),
-        m_getAccelerationFunction(getterFunction) {}
-  double GetAcceleration() override { return m_getAccelerationFunction(); }
-
-  void SetAcceleration(double acceleration) override {
-    m_setAccelerationFunction(acceleration);
-  }
-
-  bool IsWrapperInitialized() const override {
-    return m_isInitializedFunction();
-  }
-
- private:
-  std::function<bool(void)> m_isInitializedFunction;
-  std::function<void(double)> m_setAccelerationFunction;
-  std::function<double(void)> m_getAccelerationFunction;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleGyroSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleGyroSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleGyroSim.h
deleted file mode 100644
index ea69247..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleGyroSim.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-
-#include "lowfisim/GyroSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class SimpleGyroSim : public GyroSim {
- public:
-  SimpleGyroSim(std::function<void(double)>& setterFunction,
-                std::function<double(void)>& getterFunction)
-      : m_setAngleFunction(setterFunction),
-        m_getAngleFunction(getterFunction) {}
-  double GetAngle() override { return m_getAngleFunction(); }
-
-  void SetAngle(double angle) override { m_setAngleFunction(angle); }
-
- private:
-  std::function<void(double)> m_setAngleFunction;
-  std::function<double(void)> m_getAngleFunction;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponent.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponent.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponent.h
deleted file mode 100644
index 4eb5e99..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponent.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class SimulatorComponent {
- public:
-  virtual ~SimulatorComponent() = default;
-
-  virtual bool IsWrapperInitialized() const = 0;
-
-  virtual const std::string& GetDisplayName() const = 0;
-
-  virtual void SetDisplayName(const std::string& displayName) = 0;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponentBase.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponentBase.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponentBase.h
deleted file mode 100644
index c29e68c..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/SimulatorComponentBase.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include "lowfisim/SimulatorComponent.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class SimulatorComponentBase : public virtual SimulatorComponent {
- public:
-  SimulatorComponentBase() = default;
-  virtual ~SimulatorComponentBase() = default;
-
-  const std::string& GetDisplayName() const override;
-
-  void SetDisplayName(const std::string& displayName) override;
-
- private:
-  std::string m_name;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/MotorModel.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/MotorModel.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/MotorModel.h
deleted file mode 100644
index aeffcb7..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/MotorModel.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class MotorModel {
- public:
-  virtual void Reset() = 0;
-  virtual void SetVoltage(double voltage) = 0;
-  virtual void Update(double elapsedTime) = 0;
-
-  virtual double GetPosition() const = 0;
-  virtual double GetVelocity() const = 0;
-  virtual double GetAcceleration() const = 0;
-  virtual double GetCurrent() const = 0;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/SimpleMotorModel.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/SimpleMotorModel.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/SimpleMotorModel.h
deleted file mode 100644
index ccfc3b9..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/motormodel/SimpleMotorModel.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/motormodel/MotorModel.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class SimpleMotorModel : public MotorModel {
- public:
-  explicit SimpleMotorModel(double maxSpeed);
-
-  void Reset() override;
-  void SetVoltage(double voltage) override;
-  void Update(double elapsedTime) override;
-
-  double GetPosition() const override;
-  double GetVelocity() const override;
-  double GetAcceleration() const override;
-  double GetCurrent() const override;
-
- protected:
-  double m_maxSpeed;
-  double m_voltagePercentage{0};
-  double m_position{0};
-  double m_velocity{0};
-
-  static constexpr double kMaxExpectedVoltage = 12;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiAnalogGyroSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiAnalogGyroSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiAnalogGyroSim.h
deleted file mode 100644
index a5957ef..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiAnalogGyroSim.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/GyroSim.h"
-#include "simulation/AnalogGyroSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class WpiAnalogGyroSim : public GyroSim {
- public:
-  explicit WpiAnalogGyroSim(int index);
-
-  bool IsWrapperInitialized() const override;
-
-  void SetAngle(double angle) override;
-  double GetAngle() override;
-
- protected:
-  frc::sim::AnalogGyroSim m_gyroSimulator;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiEncoderSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiEncoderSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiEncoderSim.h
deleted file mode 100644
index 0c936e5..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiEncoderSim.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/EncoderSim.h"
-#include "simulation/EncoderSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class WpiEncoderSim : public EncoderSim {
- public:
-  explicit WpiEncoderSim(int index);
-  bool IsWrapperInitialized() const override;
-
-  void SetPosition(double position) override;
-  void SetVelocity(double velocity) override;
-
- protected:
-  frc::sim::EncoderSim m_encoderSimulator;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiMotorSim.h

diff --git a/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiMotorSim.h b/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiMotorSim.h
deleted file mode 100644
index 9323aec..0000000
--- a/simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiMotorSim.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "lowfisim/MotorSim.h"
-#include "lowfisim/motormodel/MotorModel.h"
-#include "simulation/PWMSim.h"
-
-namespace frc {
-namespace sim {
-namespace lowfi {
-
-class WpiMotorSim : public MotorSim {
- public:
-  explicit WpiMotorSim(int index, MotorModel& motorModelSimulator);
-  bool IsWrapperInitialized() const override;
-
-  void Update(double elapsedTime);
-  double GetPosition() const override;
-  double GetVelocity() const override;
-  double GetAcceleration() const override;
-
- private:
-  MotorModel& m_motorModelSimulation;
-  frc::sim::PWMSim m_pwmSimulator;
-
-  static constexpr double kDefaultVoltage = 12.0;
-};
-
-}  // namespace lowfi
-}  // namespace sim
-}  // namespace frc

simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp

diff --git a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp b/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp
deleted file mode 100644
index 9166de3..0000000
--- a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/MotorEncoderSimulatorTest.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/Encoder.h"
-#include "frc/Talon.h"
-#include "gtest/gtest.h"
-#include "lowfisim/MotorEncoderConnector.h"
-#include "lowfisim/motormodel/SimpleMotorModel.h"
-#include "lowfisim/wpisimulators/WpiEncoderSim.h"
-#include "lowfisim/wpisimulators/WpiMotorSim.h"
-
-TEST(MotorEncoderConnectorTest, TestWithoutDistancePerPulseFullSpeed) {
-  frc::sim::lowfi::SimpleMotorModel motorModelSim(6000);
-  frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim);
-  frc::sim::lowfi::WpiEncoderSim encoderSim(0);
-  frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim);
-
-  EXPECT_FALSE(motorSim.IsWrapperInitialized());
-  EXPECT_FALSE(encoderSim.IsWrapperInitialized());
-  frc::Talon talon{3};
-  frc::Encoder encoder{3, 1};
-  EXPECT_TRUE(motorSim.IsWrapperInitialized());
-  EXPECT_TRUE(encoderSim.IsWrapperInitialized());
-
-  talon.Set(-1);
-  motorSim.Update(1);
-  connector.Update();
-
-  // Position
-  EXPECT_EQ(-6000, encoder.Get());
-  EXPECT_DOUBLE_EQ(-6000, encoder.GetDistance());
-
-  // Velocity
-  EXPECT_DOUBLE_EQ(-1.0 / 6000, encoder.GetPeriod());
-  EXPECT_DOUBLE_EQ(-6000, encoder.GetRate());
-}
-
-TEST(MotorEncoderConnectorTest, TestWithoutDistancePerPulseRealisitcUpdate) {
-  frc::Talon talon{3};
-  frc::Encoder encoder{3, 1};
-
-  frc::sim::lowfi::SimpleMotorModel motorModelSim(6000);
-  frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim);
-  frc::sim::lowfi::WpiEncoderSim encoderSim(0);
-  frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim);
-
-  talon.Set(0.5);
-  motorSim.Update(0.02);
-  connector.Update();
-
-  // Position
-  EXPECT_EQ(60, encoder.Get());
-  EXPECT_DOUBLE_EQ(60, encoder.GetDistance());
-
-  // Velocity
-  EXPECT_DOUBLE_EQ(1.0 / 3000, encoder.GetPeriod());
-  EXPECT_DOUBLE_EQ(3000, encoder.GetRate());
-}
-
-TEST(MotorEncoderConnectorTest, TestWithDistancePerPulseFullSpeed) {
-  frc::Talon talon{3};
-  frc::Encoder encoder{3, 1};
-  encoder.SetDistancePerPulse(0.001);
-
-  frc::sim::lowfi::SimpleMotorModel motorModelSim(6000);
-  frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim);
-  frc::sim::lowfi::WpiEncoderSim encoderSim(0);
-  frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim);
-
-  talon.Set(-1);
-
-  motorSim.Update(1);
-  connector.Update();
-
-  // Position
-  EXPECT_EQ(-6000000, encoder.Get());
-  EXPECT_DOUBLE_EQ(-6000, encoder.GetDistance());
-
-  // Velocity
-  EXPECT_EQ(-1.0 / 6000, encoder.GetPeriod());
-  EXPECT_DOUBLE_EQ(-6, encoder.GetRate());
-}
-
-TEST(MotorEncoderConnectorTest, TestWithDistancePerPulseRealistic) {
-  frc::Talon talon{3};
-  frc::Encoder encoder{3, 1};
-  encoder.SetDistancePerPulse(0.001);
-
-  frc::sim::lowfi::SimpleMotorModel motorModelSim(6000);
-  frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim);
-  frc::sim::lowfi::WpiEncoderSim encoderSim(0);
-  frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim);
-
-  talon.Set(0.5);
-
-  motorSim.Update(0.02);
-  connector.Update();
-
-  // Position
-  EXPECT_EQ(60000, encoder.Get());
-  EXPECT_DOUBLE_EQ(60, encoder.GetDistance());
-
-  // Velocity
-  EXPECT_EQ(1.0 / 3000, encoder.GetPeriod());
-  EXPECT_DOUBLE_EQ(3, encoder.GetRate());
-}

simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp

diff --git a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp b/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp
deleted file mode 100644
index 21c5e7e..0000000
--- a/simulation/lowfi_simulation/src/test/native/cpp/lowfisim/motormodel/SimpleMotorModelSimulationTest.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "lowfisim/motormodel/SimpleMotorModel.h"
-
-TEST(SimpleMotorModelSimulationTest, TestSimpleModel) {
-  frc::sim::lowfi::SimpleMotorModel motorModelSim(200);
-
-  // Test forward voltage
-  motorModelSim.SetVoltage(6);
-  motorModelSim.Update(0.5);
-
-  EXPECT_DOUBLE_EQ(50, motorModelSim.GetPosition());
-  EXPECT_DOUBLE_EQ(100, motorModelSim.GetVelocity());
-
-  // Test Reset
-  motorModelSim.Reset();
-  EXPECT_DOUBLE_EQ(0, motorModelSim.GetPosition());
-  EXPECT_DOUBLE_EQ(0, motorModelSim.GetVelocity());
-
-  // Test negative voltage
-  motorModelSim.Reset();
-  motorModelSim.SetVoltage(-3);
-  motorModelSim.Update(0.06);
-
-  EXPECT_DOUBLE_EQ(-3, motorModelSim.GetPosition());
-  EXPECT_DOUBLE_EQ(-50, motorModelSim.GetVelocity());
-}

simulation/lowfi_simulation/src/test/native/cpp/main.cpp

diff --git a/simulation/lowfi_simulation/src/test/native/cpp/main.cpp b/simulation/lowfi_simulation/src/test/native/cpp/main.cpp
deleted file mode 100644
index c6b6c58..0000000
--- a/simulation/lowfi_simulation/src/test/native/cpp/main.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <hal/HALBase.h>
-
-#include "gtest/gtest.h"
-
-int main(int argc, char** argv) {
-  HAL_Initialize(500, 0);
-  ::testing::InitGoogleTest(&argc, argv);
-  int ret = RUN_ALL_TESTS();
-  return ret;
-}

styleguide/checkstyle.xml

diff --git a/styleguide/checkstyle.xml b/styleguide/checkstyle.xml
index 758cee4..eeeaf08 100644
--- a/styleguide/checkstyle.xml
+++ b/styleguide/checkstyle.xml
@@ -314,5 +314,6 @@
                 value="expected" />
     </module>
     <module name="CommentsIndentation" />
+    <module name="PackageDeclaration"/>
   </module>
 </module>

styleguide/pmd-ruleset.xml

diff --git a/styleguide/pmd-ruleset.xml b/styleguide/pmd-ruleset.xml
index 50f7cc7..57ff225 100644
--- a/styleguide/pmd-ruleset.xml
+++ b/styleguide/pmd-ruleset.xml
@@ -39,6 +39,7 @@
     <exclude name="NcssConstructorCount" />
     <exclude name="NcssCount" />
     <exclude name="NcssMethodCount" />
+    <exclude name="TooManyMethods" />
   </rule>
 
   <rule ref="category/java/errorprone.xml">

test-scripts/config.sh

diff --git a/test-scripts/config.sh b/test-scripts/config.sh
index be2587a..dce9031 100755
--- a/test-scripts/config.sh
+++ b/test-scripts/config.sh
@@ -7,7 +7,7 @@
 #*----------------------------------------------------------------------------*#
 
 # If this is changed, update the .gitignore
-# so that test results are not commited to the repo
+# so that test results are not committed to the repo
 DEFAULT_LOCAL_TEST_RESULTS_DIR=../test-reports
 
 ROBOT_ADDRESS=admin@roboRIO-190-FRC.local

test-scripts/deploy-and-run-test-on-robot.sh

diff --git a/test-scripts/deploy-and-run-test-on-robot.sh b/test-scripts/deploy-and-run-test-on-robot.sh
index ca5f59f..ba67d8b 100755
--- a/test-scripts/deploy-and-run-test-on-robot.sh
+++ b/test-scripts/deploy-and-run-test-on-robot.sh
@@ -22,7 +22,7 @@
 This script should only be run on the computer connected to the roboRIO.
 Where:
     -h    Show this help text.
-    -A    Disable language recomended arguments.
+    -A    Disable language recommended arguments.
     arg   Additional arguments to be passed to test."
 
 

test-scripts/run-tests-on-robot.sh

diff --git a/test-scripts/run-tests-on-robot.sh b/test-scripts/run-tests-on-robot.sh
index 96d10c0..457818c 100755
--- a/test-scripts/run-tests-on-robot.sh
+++ b/test-scripts/run-tests-on-robot.sh
@@ -71,7 +71,7 @@
 PARAM_COUNTER=1
 printf "Param Args ${PARAM_ARGS}\n"
 
-# Check for optional paramaters
+# Check for optional parameters
 while  getopts ':hmd:A' option $PARAM_ARGS ; do
     case "$option" in
     h)
@@ -103,7 +103,7 @@
     TEST_RUN_ARGS="${DEFAULT_ARGS} ${TEST_RUN_ARGS}"
 fi
 
-# Make sure at least two paramaters are passed or four if running with -d option
+# Make sure at least two parameters are passed or four if running with -d option
 if [[ $# -lt $PARAM_COUNTER ]]; then
     printf "Invalid arg count. Should be %s, was %s.\n" "${PARAM_COUNTER}" "$#"
     echo "$usage"

wpigui/.styleguide

diff --git a/wpigui/.styleguide b/wpigui/.styleguide
new file mode 100644
index 0000000..9a462a3
--- /dev/null
+++ b/wpigui/.styleguide
@@ -0,0 +1,30 @@
+cppHeaderFileInclude {
+  \.h$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+generatedFileExclude {
+  wpigui/src/main/native/cpp/portable-file-dialogs\.cpp$
+  wpigui/src/main/native/include/portable-file-dialogs\.h$
+}
+
+repoRootNameOverride {
+  wpigui
+}
+
+includeGuardRoots {
+  wpigui/src/main/native/cpp/
+  wpigui/src/main/native/include/
+  wpigui/src/main/native/directx11/
+  wpigui/src/main/native/metal/
+  wpigui/src/main/native/opengl3/
+}
+
+includeOtherLibs {
+  ^imgui
+  ^implot
+  ^stb
+}

wpigui/CMakeLists.txt

diff --git a/wpigui/CMakeLists.txt b/wpigui/CMakeLists.txt
new file mode 100644
index 0000000..b0dbf43
--- /dev/null
+++ b/wpigui/CMakeLists.txt
@@ -0,0 +1,51 @@
+project(wpigui)
+
+include(CompileWarnings)
+include(LinkMacOSGUI)
+
+file(GLOB wpigui_src src/main/native/cpp/*.cpp)
+file(GLOB wpigui_windows_src src/main/native/directx11/*.cpp)
+file(GLOB wpigui_mac_src src/main/native/metal/*.mm)
+file(GLOB wpigui_unix_src src/main/native/opengl3/*.cpp)
+
+add_library(wpigui STATIC ${wpigui_src})
+set_target_properties(wpigui PROPERTIES DEBUG_POSTFIX "d")
+set_property(TARGET wpigui PROPERTY POSITION_INDEPENDENT_CODE ON)
+
+set_property(TARGET wpigui PROPERTY FOLDER "libraries")
+
+wpilib_target_warnings(wpigui)
+target_link_libraries(wpigui PUBLIC imgui)
+
+target_include_directories(wpigui PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<INSTALL_INTERFACE:${include_dest}/wpigui>)
+
+if (MSVC)
+    target_sources(wpigui PRIVATE ${wpigui_windows_src})
+else()
+    if (APPLE)
+        target_compile_options(wpigui PRIVATE -fobjc-arc)
+        set_target_properties(wpigui PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore")
+        target_sources(wpigui PRIVATE ${wpigui_mac_src})
+    else()
+        target_sources(wpigui PRIVATE ${wpigui_unix_src})
+    endif()
+endif()
+
+add_executable(wpiguidev src/dev/native/cpp/main.cpp)
+wpilib_link_macos_gui(wpiguidev)
+target_link_libraries(wpiguidev wpigui)
+
+install(TARGETS wpigui EXPORT wpigui DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpigui")
+
+#if (WITH_FLAT_INSTALL)
+#    set (wpigui_config_dir ${wpilib_dest})
+#else()
+#    set (wpigui_config_dir share/wpigui)
+#endif()
+
+#configure_file(wpigui-config.cmake.in ${CMAKE_BINARY_DIR}/wpigui-config.cmake )
+#install(FILES ${CMAKE_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir})
+#install(EXPORT wpigui DESTINATION ${wpigui_config_dir})

wpigui/build.gradle

diff --git a/wpigui/build.gradle b/wpigui/build.gradle
new file mode 100644
index 0000000..8151616
--- /dev/null
+++ b/wpigui/build.gradle
@@ -0,0 +1,128 @@
+import org.gradle.internal.os.OperatingSystem
+
+if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
+
+    apply plugin: 'cpp'
+    if (OperatingSystem.current().isMacOsX()) {
+        apply plugin: 'objective-cpp'
+    }
+    apply plugin: 'visual-studio'
+    apply plugin: 'edu.wpi.first.NativeUtils'
+
+    ext {
+        nativeName = 'wpigui'
+    }
+
+    apply from: "${rootDir}/shared/config.gradle"
+
+    nativeUtils.exportsConfigs {
+        wpigui {
+            x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+                                '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+                                '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+                                '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+            x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+                                '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+                                '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+                                '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        }
+    }
+
+    model {
+        components {
+            "${nativeName}"(NativeLibrarySpec) {
+                sources {
+                    cpp {
+                        source {
+                            srcDirs "src/main/native/cpp"
+                            include '*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/main/native/include'
+                        }
+                    }
+                }
+                binaries.all {
+                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                        it.buildable = false
+                        return
+                    }
+                    if (it.targetPlatform.operatingSystem.isWindows()) {
+                        it.sources {
+                            wpiguiWindowsCpp(CppSourceSet) {
+                                source {
+                                    srcDirs 'src/main/native/directx11'
+                                    include '*.cpp'
+                                }
+                            }
+                        }
+                    } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                        it.sources {
+                            wpiguiMacObjectiveCpp(ObjectiveCppSourceSet) {
+                                source {
+                                    srcDirs 'src/main/native/metal'
+                                    include '*.mm'
+                                }
+                            }
+                        }
+                    } else {
+                        it.sources {
+                            wpiguiUnixCpp(CppSourceSet) {
+                                source {
+                                    srcDirs 'src/main/native/opengl3'
+                                    include '*.cpp'
+                                }
+                            }
+                        }
+                    }
+                    it.sources.each {
+                        it.exportedHeaders {
+                            srcDirs 'src/main/native/include'
+                        }
+                    }
+                }
+            }
+            // By default, a development executable will be generated. This is to help the case of
+            // testing specific functionality of the library.
+            "${nativeName}Dev"(NativeExecutableSpec) {
+                targetBuildTypes 'debug'
+                sources {
+                    cpp {
+                        source {
+                            srcDirs 'src/dev/native/cpp'
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/dev/native/include'
+                        }
+                    }
+                }
+                binaries.all {
+                    lib library: 'wpigui'
+                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                }
+            }
+        }
+        binaries {
+            all {
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                    it.buildable = false
+                    return
+                }
+                if (it.targetPlatform.operatingSystem.isWindows()) {
+                    it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+                } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                    it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+                } else {
+                    it.linker.args << '-lX11'
+                }
+            }
+            withType(SharedLibraryBinarySpec) {
+                buildable = false
+            }
+        }
+    }
+
+    apply from: 'publish.gradle'
+}

wpigui/publish.gradle

diff --git a/wpigui/publish.gradle b/wpigui/publish.gradle
new file mode 100644
index 0000000..3114118
--- /dev/null
+++ b/wpigui/publish.gradle
@@ -0,0 +1,71 @@
+apply plugin: 'maven-publish'
+
+def baseArtifactId = 'wpigui-cpp'
+def artifactGroupId = 'edu.wpi.first.wpigui'
+def zipBaseName = '_GROUP_edu_wpi_first_wpigui_ID_wpigui-cpp_CLS'
+
+def outputsFolder = file("$project.buildDir/outputs")
+
+task cppSourcesZip(type: Zip) {
+    destinationDirectory = outputsFolder
+    archiveBaseName = zipBaseName
+    classifier = "sources"
+
+    from(licenseFile) {
+        into '/'
+    }
+
+    from('src/main/native/cpp') {
+        into '/'
+    }
+    from('src/main/native/directx11') {
+        into '/'
+    }
+    from('src/main/native/metal') {
+        into '/'
+    }
+    from('src/main/native/opengl3') {
+        into '/'
+    }
+}
+
+task cppHeadersZip(type: Zip) {
+    destinationDirectory = outputsFolder
+    archiveBaseName = zipBaseName
+    classifier = "headers"
+
+    from(licenseFile) {
+        into '/'
+    }
+
+    from('src/main/native/include') {
+        into '/'
+    }
+}
+
+build.dependsOn cppHeadersZip
+build.dependsOn cppSourcesZip
+
+addTaskToCopyAllOutputs(cppHeadersZip)
+addTaskToCopyAllOutputs(cppSourcesZip)
+
+model {
+    publishing {
+        def wpiguiTaskList = createComponentZipTasks($.components, ['wpigui'], zipBaseName, Zip, project, includeStandardZipFormat)
+
+        publications {
+            cpp(MavenPublication) {
+                wpiguiTaskList.each {
+                    artifact it
+                }
+
+                artifact cppHeadersZip
+                artifact cppSourcesZip
+
+                artifactId = baseArtifactId
+                groupId artifactGroupId
+                version wpilibVersioning.version.get()
+            }
+        }
+    }
+}

wpigui/src/dev/native/cpp/main.cpp

diff --git a/wpigui/src/dev/native/cpp/main.cpp b/wpigui/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..abd58a1
--- /dev/null
+++ b/wpigui/src/dev/native/cpp/main.cpp
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "wpigui.h"
+
+int main() {
+  wpi::gui::CreateContext();
+  wpi::gui::Initialize("Hello World", 1024, 768);
+  wpi::gui::Main();
+}

wpigui/src/main/native/cpp/portable-file-dialogs.cpp

diff --git a/wpigui/src/main/native/cpp/portable-file-dialogs.cpp b/wpigui/src/main/native/cpp/portable-file-dialogs.cpp
new file mode 100644
index 0000000..09f5d37
--- /dev/null
+++ b/wpigui/src/main/native/cpp/portable-file-dialogs.cpp
@@ -0,0 +1,1366 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+//
+//  Portable File Dialogs
+//
+//  Copyright © 2018—2020 Sam Hocevar <sam@hocevar.net>
+//
+//  This library is free software. It comes without any warranty, to
+//  the extent permitted by applicable law. You can redistribute it
+//  and/or modify it under the terms of the Do What the **** You Want
+//  to Public License, Version 2, as published by the WTFPL Task Force.
+//  See http://www.wtfpl.net/ for more details.
+//
+
+#include "portable-file-dialogs.h"
+
+#if _WIN32
+#ifndef WIN32_LEAN_AND_MEAN
+#   define WIN32_LEAN_AND_MEAN 1
+#endif
+#include <Windows.h>
+#include <commdlg.h>
+#include <ShlObj.h>
+#include <ShObjIdl.h> // IFileDialog
+#include <shellapi.h>
+#include <strsafe.h>
+#include <future>     // std::async
+
+#elif __EMSCRIPTEN__
+#include <emscripten.h>
+
+#else
+#ifndef _POSIX_C_SOURCE
+#   define _POSIX_C_SOURCE 2 // for popen()
+#endif
+#include <cstdio>     // popen()
+#include <cstdlib>    // std::getenv()
+#include <fcntl.h>    // fcntl()
+#include <unistd.h>   // read(), pipe(), dup2()
+#include <csignal>    // ::kill, std::signal
+#include <sys/wait.h> // waitpid()
+#endif
+
+#ifdef _WIN32
+#include <set>
+#endif
+#include <iostream>
+#include <regex>
+#include <thread>
+#include <chrono>
+
+//
+// Below this are all the method implementations.
+//
+
+namespace pfd
+{
+
+namespace internal
+{
+
+class platform
+{
+public:
+#if _WIN32
+    // Helper class around LoadLibraryA() and GetProcAddress() with some safety
+    class dll
+    {
+    public:
+        explicit dll(std::string const &name);
+        ~dll();
+
+        template<typename T> class proc
+        {
+        public:
+            proc(dll const &lib, std::string const &sym)
+              : m_proc(reinterpret_cast<T *>(::GetProcAddress(lib.handle, sym.c_str())))
+            {}
+
+            operator bool() const { return m_proc != nullptr; }
+            operator T *() const { return m_proc; }
+
+        private:
+            T *m_proc;
+        };
+
+    private:
+        HMODULE handle;
+    };
+
+    // Helper class around CreateActCtx() and ActivateActCtx()
+    class new_style_context
+    {
+    public:
+        new_style_context();
+        ~new_style_context();
+
+    private:
+        HANDLE create();
+        ULONG_PTR m_cookie = 0;
+    };
+#endif
+};
+
+class executor
+{
+    friend class dialog;
+
+public:
+    // High level function to get the result of a command
+    std::string result(int *exit_code = nullptr);
+
+    // High level function to abort
+    bool kill();
+
+#if _WIN32
+    void start_func(std::function<std::string(int *)> const &fun);
+    static BOOL CALLBACK enum_windows_callback(HWND hwnd, LPARAM lParam);
+#elif __EMSCRIPTEN__
+    void start(int exit_code);
+#else
+    void start_process(std::vector<std::string> const &command);
+#endif
+
+    ~executor();
+
+protected:
+    bool ready(int timeout = default_wait_timeout);
+    void stop();
+
+private:
+    bool m_running = false;
+    std::string m_stdout;
+    int m_exit_code = -1;
+#if _WIN32
+    std::future<std::string> m_future;
+    std::set<HWND> m_windows;
+    std::condition_variable m_cond;
+    std::mutex m_mutex;
+    DWORD m_tid;
+#elif __EMSCRIPTEN__ || __NX__
+    // FIXME: do something
+#else
+    pid_t m_pid = 0;
+    int m_fd = -1;
+#endif
+};
+
+// internal free functions implementations
+
+#if _WIN32
+static inline std::wstring str2wstr(std::string const &str)
+{
+    int len = MultiByteToWideChar(CP_UTF8, 0, str.c_str(), (int)str.size(), nullptr, 0);
+    std::wstring ret(len, '\0');
+    MultiByteToWideChar(CP_UTF8, 0, str.c_str(), (int)str.size(), (LPWSTR)ret.data(), (int)ret.size());
+    return ret;
+}
+
+static inline std::string wstr2str(std::wstring const &str)
+{
+    int len = WideCharToMultiByte(CP_UTF8, 0, str.c_str(), (int)str.size(), nullptr, 0, nullptr, nullptr);
+    std::string ret(len, '\0');
+    WideCharToMultiByte(CP_UTF8, 0, str.c_str(), (int)str.size(), (LPSTR)ret.data(), (int)ret.size(), nullptr, nullptr);
+    return ret;
+}
+
+static inline bool is_vista()
+{
+    OSVERSIONINFOEXW osvi;
+    memset(&osvi, 0, sizeof(osvi));
+    DWORDLONG const mask = VerSetConditionMask(
+            VerSetConditionMask(
+                    VerSetConditionMask(
+                            0, VER_MAJORVERSION, VER_GREATER_EQUAL),
+                    VER_MINORVERSION, VER_GREATER_EQUAL),
+            VER_SERVICEPACKMAJOR, VER_GREATER_EQUAL);
+    osvi.dwOSVersionInfoSize = sizeof(osvi);
+    osvi.dwMajorVersion = HIBYTE(_WIN32_WINNT_VISTA);
+    osvi.dwMinorVersion = LOBYTE(_WIN32_WINNT_VISTA);
+    osvi.wServicePackMajor = 0;
+
+    return VerifyVersionInfoW(&osvi, VER_MAJORVERSION | VER_MINORVERSION | VER_SERVICEPACKMAJOR, mask) != FALSE;
+}
+#endif
+
+// This is necessary until C++20 which will have std::string::ends_with() etc.
+
+static inline bool ends_with(std::string const &str, std::string const &suffix)
+{
+    return suffix.size() <= str.size() &&
+        str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0;
+}
+
+static inline bool starts_with(std::string const &str, std::string const &prefix)
+{
+    return prefix.size() <= str.size() &&
+        str.compare(0, prefix.size(), prefix) == 0;
+}
+
+} // namespace internal
+
+// settings implementation
+
+settings::settings(bool resync)
+{
+    flags(flag::is_scanned) &= !resync;
+
+    if (flags(flag::is_scanned))
+        return;
+
+#if _WIN32
+    flags(flag::is_vista) = internal::is_vista();
+#elif !__APPLE__
+    flags(flag::has_zenity) = check_program("zenity");
+    flags(flag::has_matedialog) = check_program("matedialog");
+    flags(flag::has_qarma) = check_program("qarma");
+    flags(flag::has_kdialog) = check_program("kdialog");
+
+    // If multiple helpers are available, try to default to the best one
+    if (flags(flag::has_zenity) && flags(flag::has_kdialog))
+    {
+        auto desktop_name = std::getenv("XDG_SESSION_DESKTOP");
+        if (desktop_name && desktop_name == std::string("gnome"))
+            flags(flag::has_kdialog) = false;
+        else if (desktop_name && desktop_name == std::string("KDE"))
+            flags(flag::has_zenity) = false;
+    }
+#endif
+
+    flags(flag::is_scanned) = true;
+}
+
+bool settings::available()
+{
+#if _WIN32
+    return true;
+#elif __APPLE__
+    return true;
+#else
+    settings tmp;
+    return tmp.flags(flag::has_zenity) ||
+           tmp.flags(flag::has_matedialog) ||
+           tmp.flags(flag::has_qarma) ||
+           tmp.flags(flag::has_kdialog);
+#endif
+}
+
+void settings::verbose(bool value)
+{
+    settings().flags(flag::is_verbose) = value;
+}
+
+void settings::rescan()
+{
+    settings(/* resync = */ true);
+}
+
+// Check whether a program is present using “which”.
+bool settings::check_program(std::string const &program)
+{
+#if _WIN32
+    (void)program;
+    return false;
+#elif __EMSCRIPTEN__
+    (void)program;
+    return false;
+#else
+    int exit_code = -1;
+    internal::executor async;
+    async.start_process({"/bin/sh", "-c", "which " + program});
+    async.result(&exit_code);
+    return exit_code == 0;
+#endif
+}
+
+bool const &settings::flags(flag in_flag) const
+{
+    static bool flags[size_t(flag::max_flag)];
+    return flags[size_t(in_flag)];
+}
+
+bool &settings::flags(flag in_flag)
+{
+    return const_cast<bool &>(static_cast<settings const *>(this)->flags(in_flag));
+}
+
+// executor implementation
+
+std::string internal::executor::result(int *exit_code /* = nullptr */)
+{
+    stop();
+    if (exit_code)
+        *exit_code = m_exit_code;
+    return m_stdout;
+}
+
+bool internal::executor::kill()
+{
+#if _WIN32
+    if (m_future.valid())
+    {
+        // Close all windows that weren’t open when we started the future
+        auto previous_windows = m_windows;
+        EnumWindows(&enum_windows_callback, (LPARAM)this);
+        for (auto hwnd : m_windows)
+            if (previous_windows.find(hwnd) == previous_windows.end())
+                SendMessage(hwnd, WM_CLOSE, 0, 0);
+    }
+#elif __EMSCRIPTEN__ || __NX__
+    // FIXME: do something
+    (void)timeout;
+    return false; // cannot kill
+#else
+    ::kill(m_pid, SIGKILL);
+#endif
+    stop();
+    return true;
+}
+
+#if _WIN32
+BOOL CALLBACK internal::executor::enum_windows_callback(HWND hwnd, LPARAM lParam)
+{
+    auto that = (executor *)lParam;
+
+    DWORD pid;
+    auto tid = GetWindowThreadProcessId(hwnd, &pid);
+    if (tid == that->m_tid)
+        that->m_windows.insert(hwnd);
+    return TRUE;
+}
+#endif
+
+#if _WIN32
+void internal::executor::start_func(std::function<std::string(int *)> const &fun)
+{
+    stop();
+
+    auto trampoline = [fun, this]()
+    {
+        // Save our thread id so that the caller can cancel us
+        m_tid = GetCurrentThreadId();
+        EnumWindows(&enum_windows_callback, (LPARAM)this);
+        m_cond.notify_all();
+        return fun(&m_exit_code);
+    };
+
+    std::unique_lock<std::mutex> lock(m_mutex);
+    m_future = std::async(std::launch::async, trampoline);
+    m_cond.wait(lock);
+    m_running = true;
+}
+
+#elif __EMSCRIPTEN__
+void internal::executor::start(int exit_code)
+{
+    m_exit_code = exit_code;
+}
+
+#else
+void internal::executor::start_process(std::vector<std::string> const &command)
+{
+    stop();
+    m_stdout.clear();
+    m_exit_code = -1;
+
+    int in[2], out[2];
+    if (pipe(in) != 0 || pipe(out) != 0)
+        return;
+
+    m_pid = fork();
+    if (m_pid < 0)
+        return;
+
+    close(in[m_pid ? 0 : 1]);
+    close(out[m_pid ? 1 : 0]);
+
+    if (m_pid == 0)
+    {
+        dup2(in[0], STDIN_FILENO);
+        dup2(out[1], STDOUT_FILENO);
+
+        // Ignore stderr so that it doesn’t pollute the console (e.g. GTK+ errors from zenity)
+        int fd = open("/dev/null", O_WRONLY);
+        dup2(fd, STDERR_FILENO);
+        close(fd);
+
+        std::vector<char *> args;
+        std::transform(command.cbegin(), command.cend(), std::back_inserter(args),
+                       [](std::string const &s) { return const_cast<char *>(s.c_str()); });
+        args.push_back(nullptr); // null-terminate argv[]
+
+        execvp(args[0], args.data());
+        exit(1);
+    }
+
+    close(in[1]);
+    m_fd = out[0];
+    auto flags = fcntl(m_fd, F_GETFL);
+    fcntl(m_fd, F_SETFL, flags | O_NONBLOCK);
+
+    m_running = true;
+}
+#endif
+
+internal::executor::~executor()
+{
+    stop();
+}
+
+bool internal::executor::ready(int timeout /* = default_wait_timeout */)
+{
+    if (!m_running)
+        return true;
+
+#if _WIN32
+    if (m_future.valid())
+    {
+        auto status = m_future.wait_for(std::chrono::milliseconds(timeout));
+        if (status != std::future_status::ready)
+        {
+            // On Windows, we need to run the message pump. If the async
+            // thread uses a Windows API dialog, it may be attached to the
+            // main thread and waiting for messages that only we can dispatch.
+            MSG msg;
+            while (PeekMessage(&msg, nullptr, 0, 0, PM_REMOVE))
+            {
+                TranslateMessage(&msg);
+                DispatchMessage(&msg);
+            }
+            return false;
+        }
+
+        m_stdout = m_future.get();
+    }
+#elif __EMSCRIPTEN__ || __NX__
+    // FIXME: do something
+    (void)timeout;
+#else
+    char buf[BUFSIZ];
+    ssize_t received = read(m_fd, buf, BUFSIZ); // Flawfinder: ignore
+    if (received > 0)
+    {
+        m_stdout += std::string(buf, received);
+        return false;
+    }
+
+    // Reap child process if it is dead. It is possible that the system has already reaped it
+    // (this happens when the calling application handles or ignores SIG_CHLD) and results in
+    // waitpid() failing with ECHILD. Otherwise we assume the child is running and we sleep for
+    // a little while.
+    int status;
+    pid_t child = waitpid(m_pid, &status, WNOHANG);
+    if (child != m_pid && (child >= 0 || errno != ECHILD))
+    {
+        // FIXME: this happens almost always at first iteration
+        std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
+        return false;
+    }
+
+    close(m_fd);
+    m_exit_code = WEXITSTATUS(status);
+#endif
+
+    m_running = false;
+    return true;
+}
+
+void internal::executor::stop()
+{
+    // Loop until the user closes the dialog
+    while (!ready())
+    {}
+}
+
+// dll implementation
+
+#if _WIN32
+internal::platform::dll::dll(std::string const &name)
+  : handle(::LoadLibraryA(name.c_str()))
+{}
+
+internal::platform::dll::~dll()
+{
+    if (handle)
+        ::FreeLibrary(handle);
+}
+#endif // _WIN32
+
+// new_style_context implementation
+
+#if _WIN32
+internal::platform::new_style_context::new_style_context()
+{
+    // Only create one activation context for the whole app lifetime.
+    static HANDLE hctx = create();
+
+    if (hctx != INVALID_HANDLE_VALUE)
+        ActivateActCtx(hctx, &m_cookie);
+}
+
+internal::platform::new_style_context::~new_style_context()
+{
+    DeactivateActCtx(0, m_cookie);
+}
+
+HANDLE internal::platform::new_style_context::create()
+{
+    // This “hack” seems to be necessary for this code to work on windows XP.
+    // Without it, dialogs do not show and close immediately. GetError()
+    // returns 0 so I don’t know what causes this. I was not able to reproduce
+    // this behavior on Windows 7 and 10 but just in case, let it be here for
+    // those versions too.
+    // This hack is not required if other dialogs are used (they load comdlg32
+    // automatically), only if message boxes are used.
+    internal::platform::dll comdlg32("comdlg32.dll");
+
+    // Using approach as shown here: https://stackoverflow.com/a/10444161
+    UINT len = ::GetSystemDirectoryA(nullptr, 0);
+    std::string sys_dir(len, '\0');
+    ::GetSystemDirectoryA(&sys_dir[0], len);
+
+    ACTCTXA act_ctx =
+    {
+        // Do not set flag ACTCTX_FLAG_SET_PROCESS_DEFAULT, since it causes a
+        // crash with error “default context is already set”.
+        sizeof(act_ctx),
+        ACTCTX_FLAG_RESOURCE_NAME_VALID | ACTCTX_FLAG_ASSEMBLY_DIRECTORY_VALID,
+        "shell32.dll", 0, 0, sys_dir.c_str(), (LPCSTR)124,
+    };
+
+    return ::CreateActCtxA(&act_ctx);
+}
+#endif // _WIN32
+
+// dialog implementation
+
+bool internal::dialog::ready(int timeout /* = default_wait_timeout */) const
+{
+    return m_async->ready(timeout);
+}
+
+bool internal::dialog::kill() const
+{
+    return m_async->kill();
+}
+
+internal::dialog::dialog()
+  : m_async(std::make_shared<executor>())
+{
+}
+
+std::vector<std::string> internal::dialog::desktop_helper() const
+{
+#if __APPLE__
+    return { "osascript" };
+#else
+    return { flags(flag::has_zenity) ? "zenity"
+           : flags(flag::has_matedialog) ? "matedialog"
+           : flags(flag::has_qarma) ? "qarma"
+           : flags(flag::has_kdialog) ? "kdialog"
+           : "echo" };
+#endif
+}
+
+std::string internal::dialog::buttons_to_name(choice _choice)
+{
+    switch (_choice)
+    {
+        case choice::ok_cancel: return "okcancel";
+        case choice::yes_no: return "yesno";
+        case choice::yes_no_cancel: return "yesnocancel";
+        case choice::retry_cancel: return "retrycancel";
+        case choice::abort_retry_ignore: return "abortretryignore";
+        /* case choice::ok: */ default: return "ok";
+    }
+}
+
+std::string internal::dialog::get_icon_name(icon _icon)
+{
+    switch (_icon)
+    {
+        case icon::warning: return "warning";
+        case icon::error: return "error";
+        case icon::question: return "question";
+        // Zenity wants "information" but WinForms wants "info"
+        /* case icon::info: */ default:
+#if _WIN32
+            return "info";
+#else
+            return "information";
+#endif
+    }
+}
+
+// THis is only used for debugging purposes
+std::ostream& operator <<(std::ostream &s, std::vector<std::string> const &v)
+{
+    int not_first = 0;
+    for (auto &e : v)
+        s << (not_first++ ? " " : "") << e;
+    return s;
+}
+
+// Properly quote a string for Powershell: replace ' or " with '' or ""
+// FIXME: we should probably get rid of newlines!
+// FIXME: the \" sequence seems unsafe, too!
+// XXX: this is no longer used but I would like to keep it around just in case
+std::string internal::dialog::powershell_quote(std::string const &str) const
+{
+    return "'" + std::regex_replace(str, std::regex("['\"]"), "$&$&") + "'";
+}
+
+// Properly quote a string for osascript: replace \ or " with \\ or \"
+// XXX: this also used to replace ' with \' when popen was used, but it would be
+// smarter to do shell_quote(osascript_quote(...)) if this is needed again.
+std::string internal::dialog::osascript_quote(std::string const &str) const
+{
+    return "\"" + std::regex_replace(str, std::regex("[\\\\\"]"), "\\$&") + "\"";
+}
+
+// Properly quote a string for the shell: just replace ' with '\''
+// XXX: this is no longer used but I would like to keep it around just in case
+std::string internal::dialog::shell_quote(std::string const &str) const
+{
+    return "'" + std::regex_replace(str, std::regex("'"), "'\\''") + "'";
+}
+
+// file_dialog implementation
+
+class internal::file_dialog::Impl {
+ public:
+#if _WIN32
+    static int CALLBACK bffcallback(HWND hwnd, UINT uMsg, LPARAM, LPARAM pData);
+    std::string select_folder_vista(IFileDialog *ifd, bool force_path);
+
+    std::wstring m_wtitle;
+    std::wstring m_wdefault_path;
+
+    std::vector<std::string> m_vector_result;
+#endif
+};
+
+internal::file_dialog::file_dialog(type in_type,
+            std::string const &title,
+            std::string const &default_path /* = "" */,
+            std::vector<std::string> const &filters /* = {} */,
+            opt options /* = opt::none */)
+  : m_impl(std::make_shared<Impl>())
+{
+#if _WIN32
+    std::string filter_list;
+    std::regex whitespace("  *");
+    for (size_t i = 0; i + 1 < filters.size(); i += 2)
+    {
+        filter_list += filters[i] + '\0';
+        filter_list += std::regex_replace(filters[i + 1], whitespace, ";") + '\0';
+    }
+    filter_list += '\0';
+
+    m_async->start_func([this, in_type, title, default_path, filter_list,
+                         options](int *exit_code) -> std::string
+    {
+        (void)exit_code;
+        m_impl->m_wtitle = internal::str2wstr(title);
+        m_impl->m_wdefault_path = internal::str2wstr(default_path);
+        auto wfilter_list = internal::str2wstr(filter_list);
+
+        // Folder selection uses a different method
+        if (in_type == type::folder)
+        {
+            internal::platform::dll ole32("ole32.dll");
+
+            auto status = internal::platform::dll::proc<HRESULT WINAPI (LPVOID, DWORD)>(ole32, "CoInitializeEx")
+                              (nullptr, COINIT_APARTMENTTHREADED);
+            if (flags(flag::is_vista))
+            {
+                // On Vista and higher we should be able to use IFileDialog for folder selection
+                IFileDialog *ifd;
+                HRESULT hr = internal::platform::dll::proc<HRESULT WINAPI (REFCLSID, LPUNKNOWN, DWORD, REFIID, LPVOID *)>(ole32, "CoCreateInstance")
+                                 (CLSID_FileOpenDialog, nullptr, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&ifd));
+
+                // In case CoCreateInstance fails (which it should not), try legacy approach
+                if (SUCCEEDED(hr))
+                    return m_impl->select_folder_vista(ifd, options & opt::force_path);
+            }
+
+            BROWSEINFOW bi;
+            memset(&bi, 0, sizeof(bi));
+
+            bi.lpfn = &Impl::bffcallback;
+            bi.lParam = (LPARAM)m_impl.get();
+
+            if (flags(flag::is_vista))
+            {
+                // This hangs on Windows XP, as reported here:
+                // https://github.com/samhocevar/portable-file-dialogs/pull/21
+                if (status == S_OK)
+                    bi.ulFlags |= BIF_NEWDIALOGSTYLE;
+                bi.ulFlags |= BIF_EDITBOX;
+                bi.ulFlags |= BIF_STATUSTEXT;
+            }
+
+            auto *list = SHBrowseForFolderW(&bi);
+            std::string ret;
+            if (list)
+            {
+                auto buffer = new wchar_t[MAX_PATH];
+                SHGetPathFromIDListW(list, buffer);
+                internal::platform::dll::proc<void WINAPI (LPVOID)>(ole32, "CoTaskMemFree")(list);
+                ret = internal::wstr2str(buffer);
+                delete[] buffer;
+            }
+            if (status == S_OK)
+                internal::platform::dll::proc<void WINAPI ()>(ole32, "CoUninitialize")();
+            return ret;
+        }
+
+        OPENFILENAMEW ofn;
+        memset(&ofn, 0, sizeof(ofn));
+        ofn.lStructSize = sizeof(OPENFILENAMEW);
+        ofn.hwndOwner = GetActiveWindow();
+
+        ofn.lpstrFilter = wfilter_list.c_str();
+
+        auto woutput = std::wstring(MAX_PATH * 256, L'\0');
+        ofn.lpstrFile = (LPWSTR)woutput.data();
+        ofn.nMaxFile = (DWORD)woutput.size();
+        if (!m_impl->m_wdefault_path.empty())
+        {
+            // If a directory was provided, use it as the initial directory. If
+            // a valid path was provided, use it as the initial file. Otherwise,
+            // let the Windows API decide.
+            auto path_attr = GetFileAttributesW(m_impl->m_wdefault_path.c_str());
+            if (path_attr != INVALID_FILE_ATTRIBUTES && (path_attr & FILE_ATTRIBUTE_DIRECTORY))
+                ofn.lpstrInitialDir = m_impl->m_wdefault_path.c_str();
+            else if (m_impl->m_wdefault_path.size() <= woutput.size())
+                //second argument is size of buffer, not length of string
+                StringCchCopyW(ofn.lpstrFile, MAX_PATH*256+1, m_impl->m_wdefault_path.c_str());
+            else
+            {
+                ofn.lpstrFileTitle = (LPWSTR)m_impl->m_wdefault_path.data();
+                ofn.nMaxFileTitle = (DWORD)m_impl->m_wdefault_path.size();
+            }
+        }
+        ofn.lpstrTitle = m_impl->m_wtitle.c_str();
+        ofn.Flags = OFN_NOCHANGEDIR | OFN_EXPLORER;
+
+        internal::platform::dll comdlg32("comdlg32.dll");
+
+        if (in_type == type::save)
+        {
+            if (!(options & opt::force_overwrite))
+                ofn.Flags |= OFN_OVERWRITEPROMPT;
+
+            // using set context to apply new visual style (required for windows XP)
+            internal::platform::new_style_context ctx;
+
+            internal::platform::dll::proc<BOOL WINAPI (LPOPENFILENAMEW)> get_save_file_name(comdlg32, "GetSaveFileNameW");
+            if (get_save_file_name(&ofn) == 0)
+                return "";
+            return internal::wstr2str(woutput.c_str());
+        }
+
+        if (options & opt::multiselect)
+            ofn.Flags |= OFN_ALLOWMULTISELECT;
+        ofn.Flags |= OFN_PATHMUSTEXIST;
+
+        // using set context to apply new visual style (required for windows XP)
+        internal::platform::new_style_context ctx;
+
+        internal::platform::dll::proc<BOOL WINAPI (LPOPENFILENAMEW)> get_open_file_name(comdlg32, "GetOpenFileNameW");
+        if (get_open_file_name(&ofn) == 0)
+            return "";
+
+        std::string prefix;
+        for (wchar_t const *p = woutput.c_str(); *p; )
+        {
+            auto filename = internal::wstr2str(p);
+            p += filename.size();
+            // In multiselect mode, we advance p one step more and
+            // check for another filename. If there is one and the
+            // prefix is empty, it means we just read the prefix.
+            if ((options & opt::multiselect) && *++p && prefix.empty())
+            {
+                prefix = filename + "/";
+                continue;
+            }
+
+            m_impl->m_vector_result.push_back(prefix + filename);
+        }
+
+        return "";
+    });
+#else
+    auto command = desktop_helper();
+
+    if (is_osascript())
+    {
+        std::string script = "set ret to choose";
+        switch (in_type)
+        {
+            case type::save:
+                script += " file name";
+                break;
+            case type::open: default:
+                script += " file";
+                if (options & opt::multiselect)
+                    script += " with multiple selections allowed";
+                break;
+            case type::folder:
+                script += " folder";
+                break;
+        }
+
+        if (default_path.size())
+            script += " default location " + osascript_quote(default_path);
+        script += " with prompt " + osascript_quote(title);
+
+        if (in_type == type::open)
+        {
+            // Concatenate all user-provided filter patterns
+            std::string patterns;
+            for (size_t i = 0; i < filters.size() / 2; ++i)
+                patterns += " " + filters[2 * i + 1];
+
+            // Split the pattern list to check whether "*" is in there; if it
+            // is, we have to disable filters because there is no mechanism in
+            // OS X for the user to override the filter.
+            std::regex sep("\\s+");
+            std::string filter_list;
+            bool has_filter = true;
+            std::sregex_token_iterator iter(patterns.begin(), patterns.end(), sep, -1);
+            std::sregex_token_iterator end;
+            for ( ; iter != end; ++iter)
+            {
+                auto pat = iter->str();
+                if (pat == "*" || pat == "*.*")
+                    has_filter = false;
+                else if (internal::starts_with(pat, "*."))
+                    filter_list += (filter_list.size() == 0 ? "" : ",") +
+                                   osascript_quote(pat.substr(2, pat.size() - 2));
+            }
+            if (has_filter && filter_list.size() > 0)
+                script += " of type {" + filter_list + "}";
+        }
+
+        if (in_type == type::open && (options & opt::multiselect))
+        {
+            script += "\nset s to \"\"";
+            script += "\nrepeat with i in ret";
+            script += "\n  set s to s & (POSIX path of i) & \"\\n\"";
+            script += "\nend repeat";
+            script += "\ncopy s to stdout";
+        }
+        else
+        {
+            script += "\nPOSIX path of ret";
+        }
+
+        command.push_back("-e");
+        command.push_back(script);
+    }
+    else if (is_zenity())
+    {
+        command.push_back("--file-selection");
+        command.push_back("--filename=" + default_path);
+        command.push_back("--title");
+        command.push_back(title);
+        command.push_back("--separator=\n");
+
+        for (size_t i = 0; i < filters.size() / 2; ++i)
+        {
+            command.push_back("--file-filter");
+            command.push_back(filters[2 * i] + "|" + filters[2 * i + 1]);
+        }
+
+        if (in_type == type::save)
+            command.push_back("--save");
+        if (in_type == type::folder)
+            command.push_back("--directory");
+        if (!(options & opt::force_overwrite))
+            command.push_back("--confirm-overwrite");
+        if (options & opt::multiselect)
+            command.push_back("--multiple");
+    }
+    else if (is_kdialog())
+    {
+        switch (in_type)
+        {
+            case type::save: command.push_back("--getsavefilename"); break;
+            case type::open: command.push_back("--getopenfilename"); break;
+            case type::folder: command.push_back("--getexistingdirectory"); break;
+        }
+        if (options & opt::multiselect)
+            command.push_back(" --multiple");
+
+        command.push_back(default_path);
+
+        std::string filter;
+        for (size_t i = 0; i < filters.size() / 2; ++i)
+            filter += (i == 0 ? "" : " | ") + filters[2 * i] + "(" + filters[2 * i + 1] + ")";
+        command.push_back(filter);
+
+        command.push_back("--title");
+        command.push_back(title);
+    }
+
+    if (flags(flag::is_verbose))
+        std::cerr << "pfd: " << command << std::endl;
+
+    m_async->start_process(command);
+#endif
+}
+
+std::string internal::file_dialog::string_result()
+{
+#if _WIN32
+    return m_async->result();
+#else
+    auto ret = m_async->result();
+    // Strip potential trailing newline (zenity). Also strip trailing slash
+    // added by osascript for consistency with other backends.
+    while (ret.back() == '\n' || ret.back() == '/')
+        ret = ret.substr(0, ret.size() - 1);
+    return ret;
+#endif
+}
+
+std::vector<std::string> internal::file_dialog::vector_result()
+{
+#if _WIN32
+    m_async->result();
+    return m_impl->m_vector_result;
+#else
+    std::vector<std::string> ret;
+    auto result = m_async->result();
+    for (;;)
+    {
+        // Split result along newline characters
+        auto i = result.find('\n');
+        if (i == 0 || i == std::string::npos)
+            break;
+        ret.push_back(result.substr(0, i));
+        result = result.substr(i + 1, result.size());
+    }
+    return ret;
+#endif
+}
+
+#if _WIN32
+// Use a static function to pass as BFFCALLBACK for legacy folder select
+int CALLBACK internal::file_dialog::Impl::bffcallback(HWND hwnd, UINT uMsg,
+                                                       LPARAM, LPARAM pData)
+{
+    auto inst = (file_dialog::Impl *)pData;
+    switch (uMsg)
+    {
+        case BFFM_INITIALIZED:
+            SendMessage(hwnd, BFFM_SETSELECTIONW, TRUE, (LPARAM)inst->m_wdefault_path.c_str());
+            break;
+    }
+    return 0;
+}
+
+std::string internal::file_dialog::Impl::select_folder_vista(IFileDialog *ifd, bool force_path)
+{
+    std::string result;
+
+    IShellItem *folder;
+
+    // Load library at runtime so app doesn't link it at load time (which will fail on windows XP)
+    internal::platform::dll shell32("shell32.dll");
+    internal::platform::dll::proc<HRESULT WINAPI (PCWSTR, IBindCtx*, REFIID, void**)>
+        create_item(shell32, "SHCreateItemFromParsingName");
+
+    if (!create_item)
+        return "";
+
+    auto hr = create_item(m_wdefault_path.c_str(),
+                          nullptr,
+                          IID_PPV_ARGS(&folder));
+
+    // Set default folder if found. This only sets the default folder. If
+    // Windows has any info about the most recently selected folder, it
+    // will display it instead. Generally, calling SetFolder() to set the
+    // current directory “is not a good or expected user experience and
+    // should therefore be avoided”:
+    // https://docs.microsoft.com/windows/win32/api/shobjidl_core/nf-shobjidl_core-ifiledialog-setfolder
+    if (SUCCEEDED(hr))
+    {
+        if (force_path)
+            ifd->SetFolder(folder);
+        else
+            ifd->SetDefaultFolder(folder);
+        folder->Release();
+    }
+
+    // Set the dialog title and option to select folders
+    ifd->SetOptions(FOS_PICKFOLDERS);
+    ifd->SetTitle(m_wtitle.c_str());
+
+    hr = ifd->Show(GetActiveWindow());
+    if (SUCCEEDED(hr))
+    {
+        IShellItem* item;
+        hr = ifd->GetResult(&item);
+        if (SUCCEEDED(hr))
+        {
+            wchar_t* wselected = nullptr;
+            item->GetDisplayName(SIGDN_FILESYSPATH, &wselected);
+            item->Release();
+
+            if (wselected)
+            {
+                result = internal::wstr2str(std::wstring(wselected));
+                internal::platform::dll ole32("ole32.dll");
+                internal::platform::dll::proc<void WINAPI (LPVOID)>(ole32, "CoTaskMemFree")(wselected);
+            }
+        }
+    }
+
+    ifd->Release();
+
+    return result;
+}
+#endif
+
+// notify implementation
+
+notify::notify(std::string const &title,
+                      std::string const &message,
+                      icon _icon /* = icon::info */)
+{
+    if (_icon == icon::question) // Not supported by notifications
+        _icon = icon::info;
+
+#if _WIN32
+    // Use a static shared pointer for notify_icon so that we can delete
+    // it whenever we need to display a new one, and we can also wait
+    // until the program has finished running.
+    struct notify_icon_data : public NOTIFYICONDATAW
+    {
+        ~notify_icon_data() { Shell_NotifyIconW(NIM_DELETE, this); }
+    };
+
+    static std::shared_ptr<notify_icon_data> nid;
+
+    // Release the previous notification icon, if any, and allocate a new
+    // one. Note that std::make_shared() does value initialization, so there
+    // is no need to memset the structure.
+    nid = nullptr;
+    nid = std::make_shared<notify_icon_data>();
+
+    // For XP support
+    nid->cbSize = NOTIFYICONDATAW_V2_SIZE;
+    nid->hWnd = nullptr;
+    nid->uID = 0;
+
+    // Flag Description:
+    // - NIF_ICON    The hIcon member is valid.
+    // - NIF_MESSAGE The uCallbackMessage member is valid.
+    // - NIF_TIP     The szTip member is valid.
+    // - NIF_STATE   The dwState and dwStateMask members are valid.
+    // - NIF_INFO    Use a balloon ToolTip instead of a standard ToolTip. The szInfo, uTimeout, szInfoTitle, and dwInfoFlags members are valid.
+    // - NIF_GUID    Reserved.
+    nid->uFlags = NIF_MESSAGE | NIF_ICON | NIF_INFO;
+
+    // Flag Description
+    // - NIIF_ERROR     An error icon.
+    // - NIIF_INFO      An information icon.
+    // - NIIF_NONE      No icon.
+    // - NIIF_WARNING   A warning icon.
+    // - NIIF_ICON_MASK Version 6.0. Reserved.
+    // - NIIF_NOSOUND   Version 6.0. Do not play the associated sound. Applies only to balloon ToolTips
+    switch (_icon)
+    {
+        case icon::warning: nid->dwInfoFlags = NIIF_WARNING; break;
+        case icon::error: nid->dwInfoFlags = NIIF_ERROR; break;
+        /* case icon::info: */ default: nid->dwInfoFlags = NIIF_INFO; break;
+    }
+
+    ENUMRESNAMEPROC icon_enum_callback = [](HMODULE, LPCTSTR, LPTSTR lpName, LONG_PTR lParam) -> BOOL
+    {
+        ((NOTIFYICONDATAW *)lParam)->hIcon = ::LoadIcon(GetModuleHandle(nullptr), lpName);
+        return false;
+    };
+
+    nid->hIcon = ::LoadIcon(nullptr, IDI_APPLICATION);
+    ::EnumResourceNames(nullptr, RT_GROUP_ICON, icon_enum_callback, (LONG_PTR)nid.get());
+
+    nid->uTimeout = 5000;
+
+    StringCchCopyW(nid->szInfoTitle, ARRAYSIZE(nid->szInfoTitle), internal::str2wstr(title).c_str());
+    StringCchCopyW(nid->szInfo, ARRAYSIZE(nid->szInfo), internal::str2wstr(message).c_str());
+
+    // Display the new icon
+    Shell_NotifyIconW(NIM_ADD, nid.get());
+#else
+    auto command = desktop_helper();
+
+    if (is_osascript())
+    {
+        command.push_back("-e");
+        command.push_back("display notification " + osascript_quote(message) +
+                          " with title " + osascript_quote(title));
+    }
+    else if (is_zenity())
+    {
+        command.push_back("--notification");
+        command.push_back("--window-icon");
+        command.push_back(get_icon_name(_icon));
+        command.push_back("--text");
+        command.push_back(title + "\n" + message);
+    }
+    else if (is_kdialog())
+    {
+        command.push_back("--icon");
+        command.push_back(get_icon_name(_icon));
+        command.push_back("--title");
+        command.push_back(title);
+        command.push_back("--passivepopup");
+        command.push_back(message);
+        command.push_back("5");
+    }
+
+    if (flags(flag::is_verbose))
+        std::cerr << "pfd: " << command << std::endl;
+
+    m_async->start_process(command);
+#endif
+}
+
+// message implementation
+
+message::message(std::string const &title,
+                        std::string const &text,
+                        choice _choice /* = choice::ok_cancel */,
+                        icon _icon /* = icon::info */)
+{
+#if _WIN32
+    UINT style = MB_TOPMOST;
+    switch (_icon)
+    {
+        case icon::warning: style |= MB_ICONWARNING; break;
+        case icon::error: style |= MB_ICONERROR; break;
+        case icon::question: style |= MB_ICONQUESTION; break;
+        /* case icon::info: */ default: style |= MB_ICONINFORMATION; break;
+    }
+
+    switch (_choice)
+    {
+        case choice::ok_cancel: style |= MB_OKCANCEL; break;
+        case choice::yes_no: style |= MB_YESNO; break;
+        case choice::yes_no_cancel: style |= MB_YESNOCANCEL; break;
+        case choice::retry_cancel: style |= MB_RETRYCANCEL; break;
+        case choice::abort_retry_ignore: style |= MB_ABORTRETRYIGNORE; break;
+        /* case choice::ok: */ default: style |= MB_OK; break;
+    }
+
+    m_mappings[IDCANCEL] = button::cancel;
+    m_mappings[IDOK] = button::ok;
+    m_mappings[IDYES] = button::yes;
+    m_mappings[IDNO] = button::no;
+    m_mappings[IDABORT] = button::abort;
+    m_mappings[IDRETRY] = button::retry;
+    m_mappings[IDIGNORE] = button::ignore;
+
+    m_async->start_func([this, text, title, style](int* exit_code) -> std::string
+    {
+        auto wtext = internal::str2wstr(text);
+        auto wtitle = internal::str2wstr(title);
+        // using set context to apply new visual style (required for all windows versions)
+        internal::platform::new_style_context ctx;
+        *exit_code = MessageBoxW(GetActiveWindow(), wtext.c_str(), wtitle.c_str(), style);
+        return "";
+    });
+
+#elif __EMSCRIPTEN__
+    std::string full_message;
+    switch (_icon)
+    {
+        case icon::warning: full_message = "⚠️"; break;
+        case icon::error: full_message = "⛔"; break;
+        case icon::question: full_message = "❓"; break;
+        /* case icon::info: */ default: full_message = "ℹ"; break;
+    }
+
+    full_message += ' ' + title + "\n\n" + text;
+
+    // This does not really start an async task; it just passes the
+    // EM_ASM_INT return value to a fake start() function.
+    m_async->start(EM_ASM_INT(
+    {
+        if ($1)
+            return window.confirm(UTF8ToString($0)) ? 0 : -1;
+        alert(UTF8ToString($0));
+        return 0;
+    }, full_message.c_str(), _choice == choice::ok_cancel));
+#else
+    auto command = desktop_helper();
+
+    if (is_osascript())
+    {
+        std::string script = "display dialog " + osascript_quote(text) +
+                             " with title " + osascript_quote(title);
+        switch (_choice)
+        {
+            case choice::ok_cancel:
+                script += "buttons {\"OK\", \"Cancel\"}"
+                          " default button \"OK\""
+                          " cancel button \"Cancel\"";
+                m_mappings[256] = button::cancel;
+                break;
+            case choice::yes_no:
+                script += "buttons {\"Yes\", \"No\"}"
+                          " default button \"Yes\""
+                          " cancel button \"No\"";
+                m_mappings[256] = button::no;
+                break;
+            case choice::yes_no_cancel:
+                script += "buttons {\"Yes\", \"No\", \"Cancel\"}"
+                          " default button \"Yes\""
+                          " cancel button \"Cancel\"";
+                m_mappings[256] = button::cancel;
+                break;
+            case choice::retry_cancel:
+                script += "buttons {\"Retry\", \"Cancel\"}"
+                          " default button \"Retry\""
+                          " cancel button \"Cancel\"";
+                m_mappings[256] = button::cancel;
+                break;
+            case choice::abort_retry_ignore:
+                script += "buttons {\"Abort\", \"Retry\", \"Ignore\"}"
+                          " default button \"Retry\""
+                          " cancel button \"Retry\"";
+                m_mappings[256] = button::cancel;
+                break;
+            case choice::ok: default:
+                script += "buttons {\"OK\"}"
+                          " default button \"OK\""
+                          " cancel button \"OK\"";
+                m_mappings[256] = button::ok;
+                break;
+        }
+        script += " with icon ";
+        switch (_icon)
+        {
+            #define PFD_OSX_ICON(n) "alias ((path to library folder from system domain) as text " \
+                "& \"CoreServices:CoreTypes.bundle:Contents:Resources:" n ".icns\")"
+            case icon::info: default: script += PFD_OSX_ICON("ToolBarInfo"); break;
+            case icon::warning: script += "caution"; break;
+            case icon::error: script += "stop"; break;
+            case icon::question: script += PFD_OSX_ICON("GenericQuestionMarkIcon"); break;
+            #undef PFD_OSX_ICON
+        }
+
+        command.push_back("-e");
+        command.push_back(script);
+    }
+    else if (is_zenity())
+    {
+        switch (_choice)
+        {
+            case choice::ok_cancel:
+                command.insert(command.end(), { "--question", "--cancel-label=Cancel", "--ok-label=OK" }); break;
+            case choice::yes_no:
+                // Do not use standard --question because it causes “No” to return -1,
+                // which is inconsistent with the “Yes/No/Cancel” mode below.
+                command.insert(command.end(), { "--question", "--switch", "--extra-button=No", "--extra-button=Yes" }); break;
+            case choice::yes_no_cancel:
+                command.insert(command.end(), { "--question", "--switch", "--extra-button=Cancel", "--extra-button=No", "--extra-button=Yes" }); break;
+            case choice::retry_cancel:
+                command.insert(command.end(), { "--question", "--switch", "--extra-button=Cancel", "--extra-button=Retry" }); break;
+            case choice::abort_retry_ignore:
+                command.insert(command.end(), { "--question", "--switch", "--extra-button=Ignore", "--extra-button=Abort", "--extra-button=Retry" }); break;
+            case choice::ok:
+            default:
+                switch (_icon)
+                {
+                    case icon::error: command.push_back("--error"); break;
+                    case icon::warning: command.push_back("--warning"); break;
+                    default: command.push_back("--info"); break;
+                }
+        }
+
+        command.insert(command.end(), { "--title", title,
+                                        "--width=300", "--height=0", // sensible defaults
+                                        "--text", text,
+                                        "--icon-name=dialog-" + get_icon_name(_icon) });
+    }
+    else if (is_kdialog())
+    {
+        if (_choice == choice::ok)
+        {
+            switch (_icon)
+            {
+                case icon::error: command.push_back("--error"); break;
+                case icon::warning: command.push_back("--sorry"); break;
+                default: command.push_back("--msgbox"); break;
+            }
+        }
+        else
+        {
+            std::string flag = "--";
+            if (_icon == icon::warning || _icon == icon::error)
+                flag += "warning";
+            flag += "yesno";
+            if (_choice == choice::yes_no_cancel)
+                flag += "cancel";
+            command.push_back(flag);
+            if (_choice == choice::yes_no || _choice == choice::yes_no_cancel)
+            {
+                m_mappings[0] = button::yes;
+                m_mappings[256] = button::no;
+            }
+        }
+
+        command.push_back(text);
+        command.push_back("--title");
+        command.push_back(title);
+
+        // Must be after the above part
+        if (_choice == choice::ok_cancel)
+            command.insert(command.end(), { "--yes-label", "OK", "--no-label", "Cancel" });
+    }
+
+    if (flags(flag::is_verbose))
+        std::cerr << "pfd: " << command << std::endl;
+
+    m_async->start_process(command);
+#endif
+}
+
+button message::result()
+{
+    int exit_code;
+    auto ret = m_async->result(&exit_code);
+    // osascript will say "button returned:Cancel\n"
+    // and others will just say "Cancel\n"
+    if (exit_code < 0 || // this means cancel
+        internal::ends_with(ret, "Cancel\n"))
+        return button::cancel;
+    if (internal::ends_with(ret, "OK\n"))
+        return button::ok;
+    if (internal::ends_with(ret, "Yes\n"))
+        return button::yes;
+    if (internal::ends_with(ret, "No\n"))
+        return button::no;
+    if (internal::ends_with(ret, "Abort\n"))
+        return button::abort;
+    if (internal::ends_with(ret, "Retry\n"))
+        return button::retry;
+    if (internal::ends_with(ret, "Ignore\n"))
+        return button::ignore;
+    if (m_mappings.count(exit_code) != 0)
+        return m_mappings[exit_code];
+    return exit_code == 0 ? button::ok : button::cancel;
+}
+
+} // namespace pfd

wpigui/src/main/native/cpp/wpigui.cpp

diff --git a/wpigui/src/main/native/cpp/wpigui.cpp b/wpigui/src/main/native/cpp/wpigui.cpp
new file mode 100644
index 0000000..955b3f5
--- /dev/null
+++ b/wpigui/src/main/native/cpp/wpigui.cpp
@@ -0,0 +1,437 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "wpigui.h"
+
+#include <algorithm>
+#include <cstdio>
+#include <cstring>
+
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_ProggyDotted.h>
+#include <imgui_impl_glfw.h>
+#include <imgui_internal.h>
+#include <implot.h>
+#include <stb_image.h>
+
+#include "wpigui_internal.h"
+
+using namespace wpi::gui;
+
+namespace wpi {
+
+Context* gui::gContext;
+
+static void ErrorCallback(int error, const char* description) {
+  std::fprintf(stderr, "GLFW Error %d: %s\n", error, description);
+}
+
+static void WindowSizeCallback(GLFWwindow* window, int width, int height) {
+  if (!gContext->maximized) {
+    gContext->width = width;
+    gContext->height = height;
+  }
+  PlatformRenderFrame();
+}
+
+static void FramebufferSizeCallback(GLFWwindow* window, int width, int height) {
+  PlatformFramebufferSizeChanged(width, height);
+}
+
+static void WindowMaximizeCallback(GLFWwindow* window, int maximized) {
+  gContext->maximized = maximized;
+}
+
+static void WindowPosCallback(GLFWwindow* window, int xpos, int ypos) {
+  if (!gContext->maximized) {
+    gContext->xPos = xpos;
+    gContext->yPos = ypos;
+  }
+}
+
+static void* IniReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                         const char* name) {
+  if (std::strcmp(name, "GLOBAL") != 0) return nullptr;
+  return static_cast<SavedSettings*>(gContext);
+}
+
+static void IniReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                        void* entry, const char* lineStr) {
+  auto impl = static_cast<SavedSettings*>(entry);
+  const char* value = std::strchr(lineStr, '=');
+  if (!value) return;
+  ++value;
+  int num = std::atoi(value);
+  if (std::strncmp(lineStr, "width=", 6) == 0) {
+    impl->width = num;
+    impl->loadedWidthHeight = true;
+  } else if (std::strncmp(lineStr, "height=", 7) == 0) {
+    impl->height = num;
+    impl->loadedWidthHeight = true;
+  } else if (std::strncmp(lineStr, "maximized=", 10) == 0) {
+    impl->maximized = num;
+  } else if (std::strncmp(lineStr, "xpos=", 5) == 0) {
+    impl->xPos = num;
+  } else if (std::strncmp(lineStr, "ypos=", 5) == 0) {
+    impl->yPos = num;
+  } else if (std::strncmp(lineStr, "userScale=", 10) == 0) {
+    impl->userScale = num;
+  } else if (std::strncmp(lineStr, "style=", 6) == 0) {
+    impl->style = num;
+  }
+}
+
+static void IniWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                        ImGuiTextBuffer* out_buf) {
+  if (!gContext) return;
+  out_buf->appendf(
+      "[MainWindow][GLOBAL]\nwidth=%d\nheight=%d\nmaximized=%d\n"
+      "xpos=%d\nypos=%d\nuserScale=%d\nstyle=%d\n\n",
+      gContext->width, gContext->height, gContext->maximized, gContext->xPos,
+      gContext->yPos, gContext->userScale, gContext->style);
+}
+
+void gui::CreateContext() {
+  gContext = new Context;
+  AddFont("ProggyDotted", [](ImGuiIO& io, float size, const ImFontConfig* cfg) {
+    return ImGui::AddFontProggyDotted(io, size, cfg);
+  });
+  PlatformCreateContext();
+}
+
+void gui::DestroyContext() {
+  PlatformDestroyContext();
+  delete gContext;
+  gContext = nullptr;
+}
+
+bool gui::Initialize(const char* title, int width, int height) {
+  gContext->title = title;
+  gContext->width = width;
+  gContext->height = height;
+  gContext->defaultWidth = width;
+  gContext->defaultHeight = height;
+
+  // Setup window
+  glfwSetErrorCallback(ErrorCallback);
+  glfwInitHint(GLFW_JOYSTICK_HAT_BUTTONS, GLFW_FALSE);
+  PlatformGlfwInitHints();
+  if (!glfwInit()) return false;
+
+  PlatformGlfwWindowHints();
+
+  // Setup Dear ImGui context
+  IMGUI_CHECKVERSION();
+  ImGui::CreateContext();
+  ImPlot::CreateContext();
+  ImGuiIO& io = ImGui::GetIO();
+
+  // Hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = "MainWindow";
+  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+  iniHandler.ReadOpenFn = IniReadOpen;
+  iniHandler.ReadLineFn = IniReadLine;
+  iniHandler.WriteAllFn = IniWriteAll;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+
+  for (auto&& initialize : gContext->initializers) {
+    if (initialize) initialize();
+  }
+
+  // Load INI file
+  ImGui::LoadIniSettingsFromDisk(io.IniFilename);
+
+  // Set initial window settings
+  glfwWindowHint(GLFW_MAXIMIZED, gContext->maximized ? GLFW_TRUE : GLFW_FALSE);
+
+  if (gContext->width == 0 || gContext->height == 0) {
+    gContext->width = gContext->defaultWidth;
+    gContext->height = gContext->defaultHeight;
+    gContext->loadedWidthHeight = false;
+  }
+
+  float windowScale = 1.0;
+  if (!gContext->loadedWidthHeight) {
+    glfwWindowHint(GLFW_SCALE_TO_MONITOR, GLFW_TRUE);
+    // get the primary monitor work area to see if we have a reasonable initial
+    // window size; if not, maximize, and default scaling to smaller
+    if (GLFWmonitor* primary = glfwGetPrimaryMonitor()) {
+      int monWidth, monHeight;
+      glfwGetMonitorWorkarea(primary, nullptr, nullptr, &monWidth, &monHeight);
+      if (monWidth < gContext->width || monHeight < gContext->height) {
+        glfwWindowHint(GLFW_MAXIMIZED, GLFW_TRUE);
+        windowScale = (std::min)(monWidth * 1.0 / gContext->width,
+                                 monHeight * 1.0 / gContext->height);
+      }
+    }
+  }
+  if (gContext->xPos != -1 && gContext->yPos != -1)
+    glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
+
+  // Create window with graphics context
+  gContext->window =
+      glfwCreateWindow(gContext->width, gContext->height,
+                       gContext->title.c_str(), nullptr, nullptr);
+  if (!gContext->window) return false;
+
+  if (!gContext->loadedWidthHeight) {
+    if (windowScale == 1.0)
+      glfwGetWindowContentScale(gContext->window, &windowScale, nullptr);
+    // force user scale if window scale is smaller
+    if (windowScale <= 0.5)
+      gContext->userScale = 0;
+    else if (windowScale <= 0.75)
+      gContext->userScale = 1;
+    if (windowScale != 1.0) {
+      for (auto&& func : gContext->windowScalers) func(windowScale);
+    }
+  }
+
+  // Update window settings
+  if (gContext->xPos != -1 && gContext->yPos != -1) {
+    glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
+    glfwShowWindow(gContext->window);
+  }
+
+  // Set window callbacks
+  glfwGetWindowSize(gContext->window, &gContext->width, &gContext->height);
+  glfwSetWindowSizeCallback(gContext->window, WindowSizeCallback);
+  glfwSetFramebufferSizeCallback(gContext->window, FramebufferSizeCallback);
+  glfwSetWindowMaximizeCallback(gContext->window, WindowMaximizeCallback);
+  glfwSetWindowPosCallback(gContext->window, WindowPosCallback);
+
+  // Setup Dear ImGui style
+  SetStyle(static_cast<Style>(gContext->style));
+
+  // Load Fonts
+  // this range is based on 13px being the "nominal" 100% size and going from
+  // ~0.5x (7px) to ~2.0x (25px)
+  for (auto&& makeFont : gContext->makeFonts) {
+    if (makeFont.second) {
+      auto& font = gContext->fonts.emplace_back();
+      for (int i = 0; i < Font::kScaledLevels; ++i) {
+        float size = 7.0f + i * 3.0f;
+        ImFontConfig cfg;
+        std::snprintf(cfg.Name, sizeof(cfg.Name), "%s-%d", makeFont.first,
+                      static_cast<int>(size));
+        font.scaled[i] = makeFont.second(io, size, &cfg);
+      }
+    }
+  }
+
+  if (!PlatformInitRenderer()) return false;
+
+  return true;
+}
+
+void gui::Main() {
+  // Main loop
+  while (!glfwWindowShouldClose(gContext->window) && !gContext->exit) {
+    // Poll and handle events (inputs, window resize, etc.)
+    glfwPollEvents();
+    PlatformRenderFrame();
+  }
+
+  // Cleanup
+  PlatformShutdown();
+  ImGui_ImplGlfw_Shutdown();
+  ImPlot::DestroyContext();
+  ImGui::DestroyContext();
+
+  glfwDestroyWindow(gContext->window);
+  glfwTerminate();
+}
+
+void gui::CommonRenderFrame() {
+  ImGui_ImplGlfw_NewFrame();
+
+  // Start the Dear ImGui frame
+  ImGui::NewFrame();
+
+  // Scale based on OS window content scaling
+  float windowScale = 1.0;
+  glfwGetWindowContentScale(gContext->window, &windowScale, nullptr);
+  // map to closest font size: 0 = 0.5x, 1 = 0.75x, 2 = 1.0x, 3 = 1.25x,
+  // 4 = 1.5x, 5 = 1.75x, 6 = 2x
+  gContext->fontScale = std::clamp(
+      gContext->userScale + static_cast<int>((windowScale - 1.0) * 4), 0,
+      Font::kScaledLevels - 1);
+  ImGui::GetIO().FontDefault = gContext->fonts[0].scaled[gContext->fontScale];
+
+  for (size_t i = 0; i < gContext->earlyExecutors.size(); ++i) {
+    auto& execute = gContext->earlyExecutors[i];
+    if (execute) execute();
+  }
+
+  for (size_t i = 0; i < gContext->lateExecutors.size(); ++i) {
+    auto& execute = gContext->lateExecutors[i];
+    if (execute) execute();
+  }
+
+  // Rendering
+  ImGui::Render();
+}
+
+void gui::Exit() {
+  if (!gContext) return;
+  gContext->exit = true;
+}
+
+void gui::AddInit(std::function<void()> initialize) {
+  if (initialize) gContext->initializers.emplace_back(std::move(initialize));
+}
+
+void gui::AddWindowScaler(std::function<void(float scale)> windowScaler) {
+  if (windowScaler)
+    gContext->windowScalers.emplace_back(std::move(windowScaler));
+}
+
+void gui::AddEarlyExecute(std::function<void()> execute) {
+  if (execute) gContext->earlyExecutors.emplace_back(std::move(execute));
+}
+
+void gui::AddLateExecute(std::function<void()> execute) {
+  if (execute) gContext->lateExecutors.emplace_back(std::move(execute));
+}
+
+GLFWwindow* gui::GetSystemWindow() { return gContext->window; }
+
+int gui::AddFont(
+    const char* name,
+    std::function<ImFont*(ImGuiIO& io, float size, const ImFontConfig* cfg)>
+        makeFont) {
+  if (makeFont) gContext->makeFonts.emplace_back(name, std::move(makeFont));
+  return gContext->makeFonts.size() - 1;
+}
+
+ImFont* gui::GetFont(int font) {
+  return gContext->fonts[font].scaled[gContext->fontScale];
+}
+
+void gui::SetStyle(Style style) {
+  gContext->style = static_cast<int>(style);
+  switch (style) {
+    case kStyleClassic:
+      ImGui::StyleColorsClassic();
+      break;
+    case kStyleDark:
+      ImGui::StyleColorsDark();
+      break;
+    case kStyleLight:
+      ImGui::StyleColorsLight();
+      break;
+  }
+}
+
+void gui::SetClearColor(ImVec4 color) { gContext->clearColor = color; }
+
+void gui::EmitViewMenu() {
+  if (ImGui::BeginMenu("View")) {
+    if (ImGui::BeginMenu("Style")) {
+      bool selected;
+      selected = gContext->style == kStyleClassic;
+      if (ImGui::MenuItem("Classic", nullptr, &selected, true))
+        SetStyle(kStyleClassic);
+      selected = gContext->style == kStyleDark;
+      if (ImGui::MenuItem("Dark", nullptr, &selected, true))
+        SetStyle(kStyleDark);
+      selected = gContext->style == kStyleLight;
+      if (ImGui::MenuItem("Light", nullptr, &selected, true))
+        SetStyle(kStyleLight);
+      ImGui::EndMenu();
+    }
+
+    if (ImGui::BeginMenu("Zoom")) {
+      for (int i = 0; i < Font::kScaledLevels && (25 * (i + 2)) <= 200; ++i) {
+        char label[20];
+        std::snprintf(label, sizeof(label), "%d%%", 25 * (i + 2));
+        bool selected = gContext->userScale == i;
+        bool enabled = (gContext->fontScale - gContext->userScale + i) >= 0 &&
+                       (gContext->fontScale - gContext->userScale + i) <
+                           Font::kScaledLevels;
+        if (ImGui::MenuItem(label, nullptr, &selected, enabled))
+          gContext->userScale = i;
+      }
+      ImGui::EndMenu();
+    }
+    ImGui::EndMenu();
+  }
+}
+
+bool gui::UpdateTextureFromImage(ImTextureID* texture, int width, int height,
+                                 const unsigned char* data, int len) {
+  // Load from memory
+  int width2 = 0;
+  int height2 = 0;
+  unsigned char* imgData =
+      stbi_load_from_memory(data, len, &width2, &height2, nullptr, 4);
+  if (!data) return false;
+
+  if (width2 == width && height2 == height)
+    UpdateTexture(texture, kPixelRGBA, width2, height2, imgData);
+  else
+    *texture = CreateTexture(kPixelRGBA, width2, height2, imgData);
+
+  stbi_image_free(imgData);
+
+  return true;
+}
+
+bool gui::CreateTextureFromFile(const char* filename, ImTextureID* out_texture,
+                                int* out_width, int* out_height) {
+  // Load from file
+  int width = 0;
+  int height = 0;
+  unsigned char* data = stbi_load(filename, &width, &height, nullptr, 4);
+  if (!data) return false;
+
+  *out_texture = CreateTexture(kPixelRGBA, width, height, data);
+  if (out_width) *out_width = width;
+  if (out_height) *out_height = height;
+
+  stbi_image_free(data);
+
+  return true;
+}
+
+bool gui::CreateTextureFromImage(const unsigned char* data, int len,
+                                 ImTextureID* out_texture, int* out_width,
+                                 int* out_height) {
+  // Load from memory
+  int width = 0;
+  int height = 0;
+  unsigned char* imgData =
+      stbi_load_from_memory(data, len, &width, &height, nullptr, 4);
+  if (!imgData) return false;
+
+  *out_texture = CreateTexture(kPixelRGBA, width, height, imgData);
+  if (out_width) *out_width = width;
+  if (out_height) *out_height = height;
+
+  stbi_image_free(imgData);
+
+  return true;
+}
+
+void gui::MaxFit(ImVec2* min, ImVec2* max, float width, float height) {
+  float destWidth = max->x - min->x;
+  float destHeight = max->y - min->y;
+  if (width == 0 || height == 0) return;
+  if (destWidth * height > destHeight * width) {
+    float outputWidth = width * destHeight / height;
+    min->x += (destWidth - outputWidth) / 2;
+    max->x -= (destWidth - outputWidth) / 2;
+  } else {
+    float outputHeight = height * destWidth / width;
+    min->y += (destHeight - outputHeight) / 2;
+    max->y -= (destHeight - outputHeight) / 2;
+  }
+}
+
+}  // namespace wpi

wpigui/src/main/native/directx11/wpigui_directx11.cpp

diff --git a/wpigui/src/main/native/directx11/wpigui_directx11.cpp b/wpigui/src/main/native/directx11/wpigui_directx11.cpp
new file mode 100644
index 0000000..6ecf155
--- /dev/null
+++ b/wpigui/src/main/native/directx11/wpigui_directx11.cpp
@@ -0,0 +1,242 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <d3d11.h>
+
+#define GLFW_INCLUDE_NONE
+#define GLFW_EXPOSE_NATIVE_WIN32
+#include <GLFW/glfw3.h>
+#include <GLFW/glfw3native.h>
+
+#include <imgui.h>
+#include <imgui_impl_glfw.h>
+#include <imgui_impl_dx11.h>
+
+#include "wpigui.h"
+#include "wpigui_internal.h"
+
+using namespace wpi::gui;
+
+namespace {
+struct PlatformContext {
+  ID3D11Device* pd3dDevice = nullptr;
+  ID3D11DeviceContext* pd3dDeviceContext = nullptr;
+  IDXGISwapChain* pSwapChain = nullptr;
+  ID3D11RenderTargetView* mainRenderTargetView = nullptr;
+};
+}  // namespace
+
+static PlatformContext* gPlatformContext;
+static bool gPlatformValid = false;
+
+static void CreateRenderTarget() {
+  ID3D11Texture2D* pBackBuffer;
+  gPlatformContext->pSwapChain->GetBuffer(0, IID_PPV_ARGS(&pBackBuffer));
+  gPlatformContext->pd3dDevice->CreateRenderTargetView(
+      pBackBuffer, nullptr, &gPlatformContext->mainRenderTargetView);
+  pBackBuffer->Release();
+}
+
+static bool CreateDeviceD3D(HWND hWnd) {
+  // Setup swap chain
+  DXGI_SWAP_CHAIN_DESC sd;
+  ZeroMemory(&sd, sizeof(sd));
+  sd.BufferCount = 2;
+  sd.BufferDesc.Width = 0;
+  sd.BufferDesc.Height = 0;
+  sd.BufferDesc.Format = DXGI_FORMAT_R8G8B8A8_UNORM;
+  sd.BufferDesc.RefreshRate.Numerator = 60;
+  sd.BufferDesc.RefreshRate.Denominator = 1;
+  sd.Flags = DXGI_SWAP_CHAIN_FLAG_ALLOW_MODE_SWITCH;
+  sd.BufferUsage = DXGI_USAGE_RENDER_TARGET_OUTPUT;
+  sd.OutputWindow = hWnd;
+  sd.SampleDesc.Count = 1;
+  sd.SampleDesc.Quality = 0;
+  sd.Windowed = TRUE;
+  sd.SwapEffect = DXGI_SWAP_EFFECT_DISCARD;
+
+  UINT createDeviceFlags = 0;
+  // createDeviceFlags |= D3D11_CREATE_DEVICE_DEBUG;
+  D3D_FEATURE_LEVEL featureLevel;
+  const D3D_FEATURE_LEVEL featureLevelArray[2] = {
+      D3D_FEATURE_LEVEL_11_0,
+      D3D_FEATURE_LEVEL_10_0,
+  };
+  if (D3D11CreateDeviceAndSwapChain(
+          nullptr, D3D_DRIVER_TYPE_HARDWARE, nullptr, createDeviceFlags,
+          featureLevelArray, 2, D3D11_SDK_VERSION, &sd,
+          &gPlatformContext->pSwapChain, &gPlatformContext->pd3dDevice,
+          &featureLevel, &gPlatformContext->pd3dDeviceContext) != S_OK)
+    return false;
+
+  CreateRenderTarget();
+  return true;
+}
+
+static void CleanupRenderTarget() {
+  if (gPlatformContext->mainRenderTargetView) {
+    gPlatformContext->mainRenderTargetView->Release();
+    gPlatformContext->mainRenderTargetView = nullptr;
+  }
+}
+
+static void CleanupDeviceD3D() {
+  CleanupRenderTarget();
+  if (gPlatformContext->pSwapChain) {
+    gPlatformContext->pSwapChain->Release();
+    gPlatformContext->pSwapChain = nullptr;
+  }
+  if (gPlatformContext->pd3dDeviceContext) {
+    gPlatformContext->pd3dDeviceContext->Release();
+    gPlatformContext->pd3dDeviceContext = nullptr;
+  }
+  if (gPlatformContext->pd3dDevice) {
+    gPlatformContext->pd3dDevice->Release();
+    gPlatformContext->pd3dDevice = nullptr;
+  }
+}
+
+namespace wpi {
+
+void gui::PlatformCreateContext() { gPlatformContext = new PlatformContext; }
+
+void gui::PlatformDestroyContext() {
+  CleanupDeviceD3D();
+  delete gPlatformContext;
+  gPlatformContext = nullptr;
+}
+
+void gui::PlatformGlfwInitHints() {}
+
+void gui::PlatformGlfwWindowHints() {
+  glfwWindowHint(GLFW_CLIENT_API, GLFW_NO_API);
+}
+
+bool gui::PlatformInitRenderer() {
+  // Initialize Direct3D
+  if (!CreateDeviceD3D(glfwGetWin32Window(gContext->window))) {
+    CleanupDeviceD3D();
+    return false;
+  }
+
+  ImGui_ImplGlfw_InitForOpenGL(gContext->window, true);
+  ImGui_ImplDX11_Init(gPlatformContext->pd3dDevice,
+                      gPlatformContext->pd3dDeviceContext);
+
+  gPlatformValid = true;
+  return true;
+}
+
+void gui::PlatformRenderFrame() {
+  ImGui_ImplDX11_NewFrame();
+
+  CommonRenderFrame();
+
+  gPlatformContext->pd3dDeviceContext->OMSetRenderTargets(
+      1, &gPlatformContext->mainRenderTargetView, nullptr);
+  gPlatformContext->pd3dDeviceContext->ClearRenderTargetView(
+      gPlatformContext->mainRenderTargetView,
+      reinterpret_cast<float*>(&gContext->clearColor));
+  ImGui_ImplDX11_RenderDrawData(ImGui::GetDrawData());
+
+  gPlatformContext->pSwapChain->Present(1, 0);  // Present with vsync
+  // gPlatformContext->pSwapChain->Present(0, 0);  // Present without vsync
+}
+
+void gui::PlatformShutdown() {
+  gPlatformValid = false;
+  ImGui_ImplDX11_Shutdown();
+}
+
+void gui::PlatformFramebufferSizeChanged(int width, int height) {
+  if (gPlatformContext && gPlatformContext->pd3dDevice) {
+    CleanupRenderTarget();
+    gPlatformContext->pSwapChain->ResizeBuffers(0, width, height,
+                                                DXGI_FORMAT_UNKNOWN, 0);
+    CreateRenderTarget();
+  }
+}
+
+static inline DXGI_FORMAT DXPixelFormat(PixelFormat format) {
+  switch (format) {
+    case kPixelRGBA:
+      return DXGI_FORMAT_R8G8B8A8_UNORM;
+    case kPixelBGRA:
+      return DXGI_FORMAT_B8G8R8A8_UNORM;
+    default:
+      return DXGI_FORMAT_R8G8B8A8_UNORM;
+  }
+}
+
+ImTextureID gui::CreateTexture(PixelFormat format, int width, int height,
+                               const unsigned char* data) {
+  if (!gPlatformValid) return nullptr;
+
+  // Create texture
+  D3D11_TEXTURE2D_DESC desc;
+  ZeroMemory(&desc, sizeof(desc));
+  desc.Width = width;
+  desc.Height = height;
+  desc.MipLevels = 1;
+  desc.ArraySize = 1;
+  desc.Format = DXGI_FORMAT_R8G8B8A8_UNORM;
+  desc.SampleDesc.Count = 1;
+  desc.Usage = D3D11_USAGE_DEFAULT;
+  desc.BindFlags = D3D11_BIND_SHADER_RESOURCE;
+  desc.CPUAccessFlags = 0;
+
+  ID3D11Texture2D* pTexture = nullptr;
+  D3D11_SUBRESOURCE_DATA subResource;
+  subResource.pSysMem = data;
+  subResource.SysMemPitch = desc.Width * 4;
+  subResource.SysMemSlicePitch = 0;
+  gPlatformContext->pd3dDevice->CreateTexture2D(&desc, &subResource, &pTexture);
+
+  // Create texture view
+  D3D11_SHADER_RESOURCE_VIEW_DESC srvDesc;
+  ZeroMemory(&srvDesc, sizeof(srvDesc));
+  srvDesc.Format = DXGI_FORMAT_R8G8B8A8_UNORM;
+  srvDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURE2D;
+  srvDesc.Texture2D.MipLevels = desc.MipLevels;
+  srvDesc.Texture2D.MostDetailedMip = 0;
+  ID3D11ShaderResourceView* srv;
+  gPlatformContext->pd3dDevice->CreateShaderResourceView(pTexture, &srvDesc,
+                                                         &srv);
+  pTexture->Release();
+
+  return srv;
+}
+
+void gui::UpdateTexture(ImTextureID texture, PixelFormat, int width, int height,
+                        const unsigned char* data) {
+  if (!texture) return;
+
+  D3D11_BOX box;
+  box.front = 0;
+  box.back = 1;
+  box.left = 0;
+  box.right = width;
+  box.top = 0;
+  box.bottom = height;
+
+  ID3D11Resource* resource = nullptr;
+  static_cast<ID3D11ShaderResourceView*>(texture)->GetResource(&resource);
+
+  if (resource) {
+    gPlatformContext->pd3dDeviceContext->UpdateSubresource(
+        resource, 0, &box, data, width * 4, width * height * 4);
+
+    resource->Release();
+  }
+}
+
+void gui::DeleteTexture(ImTextureID texture) {
+  if (!gPlatformValid) return;
+  if (texture) static_cast<ID3D11ShaderResourceView*>(texture)->Release();
+}
+
+}  // namespace wpi

wpigui/src/main/native/include/portable-file-dialogs.h

diff --git a/wpigui/src/main/native/include/portable-file-dialogs.h b/wpigui/src/main/native/include/portable-file-dialogs.h
new file mode 100644
index 0000000..dffa71e
--- /dev/null
+++ b/wpigui/src/main/native/include/portable-file-dialogs.h
@@ -0,0 +1,249 @@
+//
+//  Portable File Dialogs
+//
+//  Copyright © 2018—2020 Sam Hocevar <sam@hocevar.net>
+//
+//  This library is free software. It comes without any warranty, to
+//  the extent permitted by applicable law. You can redistribute it
+//  and/or modify it under the terms of the Do What the **** You Want
+//  to Public License, Version 2, as published by the WTFPL Task Force.
+//  See http://www.wtfpl.net/ for more details.
+//
+
+#pragma once
+
+#include <string>   // std::string
+#include <memory>   // std::shared_ptr
+#include <map>      // std::map
+#include <vector>
+
+namespace pfd
+{
+
+enum class button
+{
+    cancel = -1,
+    ok,
+    yes,
+    no,
+    abort,
+    retry,
+    ignore,
+};
+
+enum class choice
+{
+    ok = 0,
+    ok_cancel,
+    yes_no,
+    yes_no_cancel,
+    retry_cancel,
+    abort_retry_ignore,
+};
+
+enum class icon
+{
+    info = 0,
+    warning,
+    error,
+    question,
+};
+
+// Additional option flags for various dialog constructors
+enum class opt : uint8_t
+{
+    none = 0,
+    // For file open, allow multiselect.
+    multiselect     = 0x1,
+    // For file save, force overwrite and disable the confirmation dialog.
+    force_overwrite = 0x2,
+    // For folder select, force path to be the provided argument instead
+    // of the last opened directory, which is the Microsoft-recommended,
+    // user-friendly behaviour.
+    force_path      = 0x4,
+};
+
+inline opt operator |(opt a, opt b) { return opt(uint8_t(a) | uint8_t(b)); }
+inline bool operator &(opt a, opt b) { return bool(uint8_t(a) & uint8_t(b)); }
+
+// The settings class, only exposing to the user a way to set verbose mode
+// and to force a rescan of installed desktop helpers (zenity, kdialog…).
+class settings
+{
+public:
+    static bool available();
+
+    static void verbose(bool value);
+    static void rescan();
+
+protected:
+    explicit settings(bool resync = false);
+
+    bool check_program(std::string const &program);
+
+    constexpr bool is_osascript() const {
+#if __APPLE__
+        return true;
+#else
+        return false;
+#endif
+    }
+    bool is_zenity() const {
+        return flags(flag::has_zenity) ||
+               flags(flag::has_matedialog) ||
+               flags(flag::has_qarma);
+    }
+    bool is_kdialog() const { return flags(flag::has_kdialog); }
+
+    enum class flag
+    {
+        is_scanned = 0,
+        is_verbose,
+
+        has_zenity,
+        has_matedialog,
+        has_qarma,
+        has_kdialog,
+        is_vista,
+
+        max_flag,
+    };
+
+    // Static array of flags for internal state
+    bool const &flags(flag in_flag) const;
+
+    // Non-const getter for the static array of flags
+    bool &flags(flag in_flag);
+};
+
+// Internal classes, not to be used by client applications
+namespace internal
+{
+
+// Process wait timeout, in milliseconds
+constexpr int default_wait_timeout = 20;
+
+class executor;
+
+class dialog : protected settings
+{
+public:
+    bool ready(int timeout = default_wait_timeout) const;
+    bool kill() const;
+
+protected:
+    explicit dialog();
+
+    std::vector<std::string> desktop_helper() const;
+    static std::string buttons_to_name(choice _choice);
+    static std::string get_icon_name(icon _icon);
+
+    std::string powershell_quote(std::string const &str) const;
+    std::string osascript_quote(std::string const &str) const;
+    std::string shell_quote(std::string const &str) const;
+
+    // Keep handle to executing command
+    std::shared_ptr<executor> m_async;
+};
+
+class file_dialog : public dialog
+{
+protected:
+    enum type
+    {
+        open,
+        save,
+        folder,
+    };
+
+    file_dialog(type in_type,
+                std::string const &title,
+                std::string const &default_path = "",
+                std::vector<std::string> const &filters = {},
+                opt options = opt::none);
+
+protected:
+    std::string string_result();
+    std::vector<std::string> vector_result();
+
+    class Impl;
+    std::shared_ptr<Impl> m_impl;
+};
+
+} // namespace internal
+
+//
+// The notify widget
+//
+
+class notify : public internal::dialog
+{
+public:
+    notify(std::string const &title,
+           std::string const &message,
+           icon _icon = icon::info);
+};
+
+//
+// The message widget
+//
+
+class message : public internal::dialog
+{
+public:
+    message(std::string const &title,
+            std::string const &text,
+            choice _choice = choice::ok_cancel,
+            icon _icon = icon::info);
+
+    button result();
+
+private:
+    // Some extra logic to map the exit code to button number
+    std::map<int, button> m_mappings;
+};
+
+//
+// The open_file, save_file, and open_folder widgets
+//
+
+class open_file : public internal::file_dialog
+{
+public:
+    open_file(std::string const &title,
+              std::string const &default_path = "",
+              std::vector<std::string> const &filters = { "All Files", "*" },
+              opt options = opt::none)
+      : file_dialog(type::open, title, default_path, filters, options)
+    {}
+
+    std::vector<std::string> result() { return vector_result(); }
+};
+
+class save_file : public internal::file_dialog
+{
+public:
+    save_file(std::string const &title,
+              std::string const &default_path = "",
+              std::vector<std::string> const &filters = { "All Files", "*" },
+              opt options = opt::none)
+      : file_dialog(type::save, title, default_path, filters, options)
+    {}
+
+    std::string result() { return string_result(); }
+};
+
+class select_folder : public internal::file_dialog
+{
+public:
+    select_folder(std::string const &title,
+                  std::string const &default_path = "",
+                  opt options = opt::none)
+      : file_dialog(type::folder, title, default_path, {}, options)
+    {}
+
+    std::string result() { return string_result(); }
+};
+
+} // namespace pfd
+

wpigui/src/main/native/include/wpigui.h

diff --git a/wpigui/src/main/native/include/wpigui.h b/wpigui/src/main/native/include/wpigui.h
new file mode 100644
index 0000000..c1dbc15
--- /dev/null
+++ b/wpigui/src/main/native/include/wpigui.h
@@ -0,0 +1,379 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include <imgui.h>
+
+extern "C" struct GLFWwindow;
+
+namespace wpi::gui {
+
+/**
+ * Creates GUI context.  Must be called prior to calling any other functions.
+ */
+void CreateContext();
+
+/**
+ * Destroys GUI context.
+ */
+void DestroyContext();
+
+/**
+ * Initializes the GUI.
+ *
+ * @param title main application window title
+ * @param width main application window width
+ * @param height main application window height
+ */
+bool Initialize(const char* title, int width, int height);
+
+/**
+ * Runs main GUI loop.  On some OS'es this must be called from the main thread.
+ * Does not return until Exit() is called.
+ */
+void Main();
+
+/**
+ * Exits main GUI loop when current loop iteration finishes.
+ * Safe to call from any thread, including from within main GUI loop.
+ */
+void Exit();
+
+/**
+ * Adds initializer to GUI.  The passed function is called once, immediately
+ * after the GUI (both GLFW and Dear ImGui) are initialized in Initialize().
+ * To have any effect, must be called prior to Initialize().
+ *
+ * @param initialize initialization function
+ */
+void AddInit(std::function<void()> initialize);
+
+/**
+ * Adds window scaler function.  The passed function is called once during
+ * Initialize() if the window scale is not 1.0.  To have any effect, must
+ * be called prior to Initialize().
+ *
+ * @param windowScaler window scaler function
+ */
+void AddWindowScaler(std::function<void(float scale)> windowScaler);
+
+/**
+ * Adds per-frame executor to GUI.  The passed function is called on each
+ * Dear ImGui frame prior to any of the late execute functions.
+ *
+ * @param execute frame execution function
+ */
+void AddEarlyExecute(std::function<void()> execute);
+
+/**
+ * Adds per-frame executor to GUI.  The passed function is called on each
+ * Dear ImGui frame after all of the early execute functions.
+ *
+ * @param execute frame execution function
+ */
+void AddLateExecute(std::function<void()> execute);
+
+/**
+ * Gets GLFW window handle.
+ */
+GLFWwindow* GetSystemWindow();
+
+/**
+ * Adds a font to the GUI.  The passed function is called during
+ * initialization as many times as necessary to create a range of sizes.
+ *
+ * @param name font name
+ * @param makeFont font creation / loader function
+ * @return Font index for later use with GetFont()
+ */
+int AddFont(
+    const char* name,
+    std::function<ImFont*(ImGuiIO& io, float size, const ImFontConfig* cfg)>
+        makeFont);
+
+/**
+ * Gets a font added with AddFont() with the appropriate font size for
+ * the current scaling of the GUI.
+ *
+ * @param font font index returned by AddFont()
+ * @return Font pointer
+ */
+ImFont* GetFont(int font);
+
+enum Style { kStyleClassic = 0, kStyleDark, kStyleLight };
+
+/**
+ * Sets the ImGui style.  Using this function makes this setting persistent.
+ *
+ * @param style Style
+ */
+void SetStyle(Style style);
+
+/**
+ * Sets the clear (background) color.
+ *
+ * @param color Color
+ */
+void SetClearColor(ImVec4 color);
+
+/**
+ * Emits a View menu (e.g. for a main menu bar) that allows setting of
+ * style and zoom.  Internally starts with ImGui::BeginMenu("View").
+ */
+void EmitViewMenu();
+
+/**
+ * Pixel formats for texture pixel data.
+ */
+enum PixelFormat { kPixelRGBA, kPixelBGRA };
+
+/**
+ * Creates a texture from pixel data.
+ *
+ * @param format pixel format
+ * @param width image width
+ * @param height image height
+ * @param data pixel data
+ * @return Texture
+ */
+ImTextureID CreateTexture(PixelFormat format, int width, int height,
+                          const unsigned char* data);
+
+/**
+ * Updates a texture from pixel data.
+ * The passed-in width and height must match the width and height of the
+ * texture.
+ *
+ * @param texture texture
+ * @param format pixel format
+ * @param width texture width
+ * @param height texture height
+ * @param data pixel data
+ */
+void UpdateTexture(ImTextureID texture, PixelFormat format, int width,
+                   int height, const unsigned char* data);
+
+/**
+ * Updates a texture from image data.
+ * The pixel format of the texture must be RGBA.  The passed-in width and
+ * height must match the width and height of the texture.  If the width and
+ * height of the image differ from the passed-in width and height, a new
+ * texture is created (note this may be inefficient).
+ *
+ * @param texture texture (pointer, may be updated)
+ * @param width texture width
+ * @param height texture height
+ * @param data image data
+ * @param len image data length
+ *
+ * @return True on success, false on failure.
+ */
+bool UpdateTextureFromImage(ImTextureID* texture, int width, int height,
+                            const unsigned char* data, int len);
+
+/**
+ * Creates a texture from an image file.
+ *
+ * @param filename filename
+ * @param out_texture texture (output)
+ * @param out_width image width (output)
+ * @param out_height image height (output)
+ * @return True on success, false on failure.
+ */
+bool CreateTextureFromFile(const char* filename, ImTextureID* out_texture,
+                           int* out_width, int* out_height);
+
+/**
+ * Creates a texture from image data.
+ *
+ * @param data image data
+ * @param len image data length
+ * @param out_texture texture (output)
+ * @param out_width image width (output)
+ * @param out_height image height (output)
+ * @return True on success, false on failure.
+ */
+bool CreateTextureFromImage(const unsigned char* data, int len,
+                            ImTextureID* out_texture, int* out_width,
+                            int* out_height);
+
+/**
+ * Deletes a texture.
+ *
+ * @param texture texture
+ */
+void DeleteTexture(ImTextureID texture);
+
+/**
+ * RAII wrapper around ImTextureID.  Also keeps track of width, height, and
+ * pixel format.
+ */
+class Texture {
+ public:
+  Texture() = default;
+
+  /**
+   * Constructs a texture from pixel data.
+   *
+   * @param format pixel format
+   * @param width image width
+   * @param height image height
+   * @param data pixel data
+   */
+  Texture(PixelFormat format, int width, int height, const unsigned char* data)
+      : m_format{format}, m_width{width}, m_height{height} {
+    m_texture = CreateTexture(format, width, height, data);
+  }
+
+  Texture(const Texture&) = delete;
+  Texture(Texture&& oth)
+      : m_texture{oth.m_texture},
+        m_format{oth.m_format},
+        m_width{oth.m_width},
+        m_height{oth.m_height} {
+    oth.m_texture = 0;
+  }
+
+  Texture& operator=(const Texture&) = delete;
+  Texture& operator=(Texture&& oth) {
+    if (m_texture) DeleteTexture(m_texture);
+    m_texture = oth.m_texture;
+    oth.m_texture = 0;
+    m_format = oth.m_format;
+    m_width = oth.m_width;
+    m_height = oth.m_height;
+    return *this;
+  }
+
+  ~Texture() {
+    if (m_texture) DeleteTexture(m_texture);
+  }
+
+  /**
+   * Evaluates to true if the texture is valid.
+   */
+  explicit operator bool() const { return m_texture; }
+
+  /**
+   * Implicit conversion to ImTextureID.
+   */
+  operator ImTextureID() const { return m_texture; }
+
+  /**
+   * Gets the texture pixel format.
+   *
+   * @return pixel format
+   */
+  PixelFormat GetFormat() const { return m_format; }
+
+  /**
+   * Gets the texture width.
+   *
+   * @return width
+   */
+  int GetWidth() const { return m_width; }
+
+  /**
+   * Gets the texture height.
+   *
+   * @return height
+   */
+  int GetHeight() const { return m_height; }
+
+  /**
+   * Updates the texture from pixel data.
+   * The image data size and format is assumed to match that of the texture.
+   *
+   * @param format pixel format
+   * @param data pixel data
+   */
+  void Update(const unsigned char* data) {
+    UpdateTexture(m_texture, m_format, m_width, m_height, data);
+  }
+
+  /**
+   * Updates the texture from image data.
+   * The pixel format of the texture must be RGBA.  If the width and height of
+   * the image differ from the texture width and height, a new texture is
+   * created (note this may be inefficient).
+   *
+   * @param data image data
+   * @param len image data length
+   *
+   * @return True on success, false on failure.
+   */
+  bool UpdateFromImage(const unsigned char* data, int len) {
+    return UpdateTextureFromImage(&m_texture, m_width, m_height, data, len);
+  }
+
+  /**
+   * Creates a texture by loading an image file.
+   *
+   * @param filename filename
+   *
+   * @return Texture, or invalid (empty) texture on failure.
+   */
+  static Texture CreateFromFile(const char* filename) {
+    Texture texture;
+    if (!CreateTextureFromFile(filename, &texture.m_texture, &texture.m_width,
+                               &texture.m_height))
+      return {};
+    return texture;
+  }
+
+  /**
+   * Creates a texture from image data.
+   *
+   * @param data image data
+   * @param len image data length
+   *
+   * @return Texture, or invalid (empty) texture on failure.
+   */
+  static Texture CreateFromImage(const unsigned char* data, int len) {
+    Texture texture;
+    if (!CreateTextureFromImage(data, len, &texture.m_texture, &texture.m_width,
+                                &texture.m_height))
+      return {};
+    return texture;
+  }
+
+ private:
+  ImTextureID m_texture = nullptr;
+  PixelFormat m_format = kPixelRGBA;
+  int m_width = 0;
+  int m_height = 0;
+};
+
+/**
+ * Get square of distance between two ImVec2's.
+ *
+ * @param a first ImVec
+ * @param b second ImVec
+ *
+ * @return Distance^2 between a and b.
+ */
+inline float GetDistSquared(const ImVec2& a, const ImVec2& b) {
+  float deltaX = b.x - a.x;
+  float deltaY = b.y - a.y;
+  return deltaX * deltaX + deltaY * deltaY;
+}
+
+/**
+ * Maximize fit in "window" while preserving aspect ratio.  Min and max
+ * passed-in values are modified such that the object maximally fits.
+ *
+ * @param min upper left corner of window (modified to fit)
+ * @param max lower right corner of window (modified to fit)
+ * @param width width of object to fit
+ * @param height height of object to fit
+ */
+void MaxFit(ImVec2* min, ImVec2* max, float width, float height);
+
+}  // namespace wpi::gui

wpigui/src/main/native/include/wpigui_internal.h

diff --git a/wpigui/src/main/native/include/wpigui_internal.h b/wpigui/src/main/native/include/wpigui_internal.h
new file mode 100644
index 0000000..9a2c70f
--- /dev/null
+++ b/wpigui/src/main/native/include/wpigui_internal.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <functional>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+
+namespace wpi::gui {
+
+struct SavedSettings {
+  bool loadedWidthHeight = false;
+  int width;
+  int height;
+  int maximized = 0;
+  int xPos = -1;
+  int yPos = -1;
+  int userScale = 2;
+  int style = 0;
+};
+
+struct Font {
+  static constexpr int kScaledLevels = 9;
+  ImFont* scaled[kScaledLevels];
+};
+
+struct Context : public SavedSettings {
+  std::atomic_bool exit{false};
+
+  std::string title;
+  int defaultWidth;
+  int defaultHeight;
+
+  GLFWwindow* window = nullptr;
+
+  std::vector<std::function<void()>> initializers;
+  std::vector<std::function<void(float scale)>> windowScalers;
+  std::vector<std::pair<
+      const char*,
+      std::function<ImFont*(ImGuiIO& io, float size, const ImFontConfig* cfg)>>>
+      makeFonts;
+
+  ImVec4 clearColor = ImVec4(0.45f, 0.55f, 0.60f, 1.00f);
+  std::vector<std::function<void()>> earlyExecutors;
+  std::vector<std::function<void()>> lateExecutors;
+
+  int fontScale = 2;  // updated by main loop
+  std::vector<Font> fonts;
+};
+
+extern Context* gContext;
+
+void PlatformCreateContext();
+void PlatformDestroyContext();
+void PlatformGlfwInitHints();
+void PlatformGlfwWindowHints();
+bool PlatformInitRenderer();
+void PlatformRenderFrame();
+void PlatformShutdown();
+void PlatformFramebufferSizeChanged(int width, int height);
+
+void CommonRenderFrame();
+
+}  // namespace wpi::gui

wpigui/src/main/native/metal/wpigui_metal.mm

diff --git a/wpigui/src/main/native/metal/wpigui_metal.mm b/wpigui/src/main/native/metal/wpigui_metal.mm
new file mode 100644
index 0000000..cf46247
--- /dev/null
+++ b/wpigui/src/main/native/metal/wpigui_metal.mm
@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#define GLFW_INCLUDE_NONE
+#define GLFW_EXPOSE_NATIVE_COCOA
+#include <GLFW/glfw3.h>
+#include <GLFW/glfw3native.h>
+
+#import <Metal/Metal.h>
+#import <QuartzCore/QuartzCore.h>
+
+#include <imgui.h>
+#include <imgui_impl_glfw.h>
+#include <imgui_impl_metal.h>
+
+#include "wpigui.h"
+#include "wpigui_internal.h"
+
+using namespace wpi::gui;
+
+namespace {
+struct PlatformContext {
+  CAMetalLayer* layer;
+  MTLRenderPassDescriptor *renderPassDescriptor;
+  id <MTLCommandQueue> commandQueue;
+};
+}  // namespace
+
+static PlatformContext* gPlatformContext;
+static bool gPlatformValid = false;
+
+namespace wpi {
+
+void gui::PlatformCreateContext() {
+  gPlatformContext = new PlatformContext;
+}
+
+void gui::PlatformDestroyContext() {
+  delete gPlatformContext;
+  gPlatformContext = nullptr;
+}
+
+void gui::PlatformGlfwInitHints() {}
+
+void gui::PlatformGlfwWindowHints() {
+  glfwWindowHint(GLFW_CLIENT_API, GLFW_NO_API);
+}
+
+bool gui::PlatformInitRenderer() {
+  id <MTLDevice> device = MTLCreateSystemDefaultDevice();
+  gPlatformContext->commandQueue = [device newCommandQueue];
+
+  ImGui_ImplGlfw_InitForOpenGL(gContext->window, true);
+  ImGui_ImplMetal_Init(device);
+
+  NSWindow *nswin = glfwGetCocoaWindow(gContext->window);
+  gPlatformContext->layer = [CAMetalLayer layer];
+  gPlatformContext->layer.device = device;
+  gPlatformContext->layer.pixelFormat = MTLPixelFormatBGRA8Unorm;
+  nswin.contentView.layer = gPlatformContext->layer;
+  nswin.contentView.wantsLayer = YES;
+
+  gPlatformContext->renderPassDescriptor = [MTLRenderPassDescriptor new];
+
+  gPlatformValid = true;
+  return true;
+}
+
+void gui::PlatformRenderFrame() {
+  @autoreleasepool
+  {
+    int width, height;
+    glfwGetFramebufferSize(gContext->window, &width, &height);
+    gPlatformContext->layer.drawableSize = CGSizeMake(width, height);
+    id<CAMetalDrawable> drawable = [gPlatformContext->layer nextDrawable];
+
+    id<MTLCommandBuffer> commandBuffer = [gPlatformContext->commandQueue commandBuffer];
+    auto renderPassDescriptor = gPlatformContext->renderPassDescriptor;
+    renderPassDescriptor.colorAttachments[0].clearColor = MTLClearColorMake(gContext->clearColor.x, gContext->clearColor.y, gContext->clearColor.z, gContext->clearColor.w);
+    renderPassDescriptor.colorAttachments[0].texture = drawable.texture;
+    renderPassDescriptor.colorAttachments[0].loadAction = MTLLoadActionClear;
+    renderPassDescriptor.colorAttachments[0].storeAction = MTLStoreActionStore;
+    id <MTLRenderCommandEncoder> renderEncoder = [commandBuffer renderCommandEncoderWithDescriptor:renderPassDescriptor];
+    [renderEncoder pushDebugGroup:@"WPI GUI"];
+
+    // Start the Dear ImGui frame
+    ImGui_ImplMetal_NewFrame(renderPassDescriptor);
+
+    CommonRenderFrame();
+
+    ImGui_ImplMetal_RenderDrawData(ImGui::GetDrawData(), commandBuffer, renderEncoder);
+
+    [renderEncoder popDebugGroup];
+    [renderEncoder endEncoding];
+
+    [commandBuffer presentDrawable:drawable];
+    [commandBuffer commit];
+  }
+}
+
+void gui::PlatformShutdown() {
+  ImGui_ImplMetal_Shutdown();
+  gPlatformValid = false;
+}
+
+void gui::PlatformFramebufferSizeChanged(int, int) {}
+
+static inline MTLPixelFormat MetalPixelFormat(PixelFormat format) {
+  switch (format) {
+    case kPixelRGBA:
+      return MTLPixelFormatRGBA8Unorm;
+    case kPixelBGRA:
+      return MTLPixelFormatBGRA8Unorm;
+    default:
+      return MTLPixelFormatRGBA8Unorm;
+  }
+}
+
+ImTextureID gui::CreateTexture(PixelFormat format, int width, int height,
+                               const unsigned char* data) {
+  if (!gPlatformValid) return nullptr;
+
+  MTLPixelFormat fmt = MetalPixelFormat(format);
+  MTLTextureDescriptor *textureDescriptor = [MTLTextureDescriptor texture2DDescriptorWithPixelFormat:fmt width:width height:height mipmapped:NO];
+  textureDescriptor.usage = MTLTextureUsageShaderRead;
+#if TARGET_OS_OSX
+  textureDescriptor.storageMode = MTLStorageModeManaged;
+#else
+  textureDescriptor.storageMode = MTLStorageModeShared;
+#endif
+  id <MTLTexture> texture = [gPlatformContext->layer.device newTextureWithDescriptor:textureDescriptor];
+  [texture replaceRegion:MTLRegionMake2D(0, 0, width, height) mipmapLevel:0 withBytes:data bytesPerRow:width * 4];
+
+  return (__bridge_retained void *)texture;
+}
+
+void gui::UpdateTexture(ImTextureID texture, PixelFormat, int width,
+                        int height, const unsigned char* data) {
+  if (!texture) return;
+  id <MTLTexture> mtlTexture = (__bridge id <MTLTexture>)texture;
+  [mtlTexture replaceRegion:MTLRegionMake2D(0, 0, width, height) mipmapLevel:0 withBytes:data bytesPerRow:width * 4];
+}
+
+void gui::DeleteTexture(ImTextureID texture) {
+  if (!gPlatformValid || !texture) return;
+  id <MTLTexture> mtlTexture = (__bridge_transfer id <MTLTexture>)texture;
+  (void)mtlTexture;
+}
+
+}  // namespace wpi

wpigui/src/main/native/opengl3/wpigui_opengl3.cpp

diff --git a/wpigui/src/main/native/opengl3/wpigui_opengl3.cpp b/wpigui/src/main/native/opengl3/wpigui_opengl3.cpp
new file mode 100644
index 0000000..bf14700
--- /dev/null
+++ b/wpigui/src/main/native/opengl3/wpigui_opengl3.cpp
@@ -0,0 +1,151 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cstdio>
+
+#include <GL/gl3w.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw.h>
+#include <imgui_impl_opengl3.h>
+
+#include "wpigui.h"
+#include "wpigui_internal.h"
+
+using namespace wpi::gui;
+
+static bool gPlatformValid = false;
+
+namespace wpi {
+
+void gui::PlatformCreateContext() {}
+
+void gui::PlatformDestroyContext() {}
+
+void gui::PlatformGlfwInitHints() {}
+
+void gui::PlatformGlfwWindowHints() {
+  // Decide GL versions
+#if __APPLE__
+  // GL 3.2 + GLSL 150
+  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
+  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);
+  glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);  // 3.2+ only
+  glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);  // Required on Mac
+  glfwWindowHint(GLFW_COCOA_GRAPHICS_SWITCHING, GLFW_TRUE);
+#else
+  // GL 3.0 + GLSL 130
+  glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
+  glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
+  // glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);  // 3.2+
+  // glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); // 3.0+
+#endif
+
+  // enable 4xMSAA
+  glfwWindowHint(GLFW_SAMPLES, 4);
+}
+
+bool gui::PlatformInitRenderer() {
+  glfwMakeContextCurrent(gContext->window);
+
+  glfwSwapInterval(1);  // Enable vsync
+
+  // Initialize OpenGL loader
+  if (gl3wInit() != 0) {
+    std::fprintf(stderr, "Failed to initialize OpenGL loader!\n");
+    return false;
+  }
+
+  // Turn on multisampling
+  glEnable(GL_MULTISAMPLE);
+
+  // Setup Platform/Renderer bindings
+  ImGui_ImplGlfw_InitForOpenGL(gContext->window, true);
+
+  // Decide GLSL versions
+#if __APPLE__
+  const char* glsl_version = "#version 150";
+#else
+  const char* glsl_version = "#version 130";
+#endif
+  ImGui_ImplOpenGL3_Init(glsl_version);
+
+  gPlatformValid = true;
+  return true;
+}
+
+void gui::PlatformRenderFrame() {
+  ImGui_ImplOpenGL3_NewFrame();
+
+  CommonRenderFrame();
+
+  int display_w, display_h;
+  glfwGetFramebufferSize(gContext->window, &display_w, &display_h);
+  glViewport(0, 0, display_w, display_h);
+  glClearColor(gContext->clearColor.x, gContext->clearColor.y,
+               gContext->clearColor.z, gContext->clearColor.w);
+  glClear(GL_COLOR_BUFFER_BIT);
+  ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData());
+
+  glfwSwapBuffers(gContext->window);
+}
+
+void gui::PlatformShutdown() {
+  gPlatformValid = false;
+  ImGui_ImplOpenGL3_Shutdown();
+}
+
+void gui::PlatformFramebufferSizeChanged(int width, int height) {}
+
+static inline GLenum GLPixelFormat(PixelFormat format) {
+  switch (format) {
+    case kPixelRGBA:
+      return GL_RGBA;
+    case kPixelBGRA:
+      return GL_BGRA;
+    default:
+      return GL_RGBA;
+  }
+}
+
+ImTextureID gui::CreateTexture(PixelFormat format, int width, int height,
+                               const unsigned char* data) {
+  if (!gPlatformValid) return nullptr;
+
+  // Create a OpenGL texture identifier
+  GLuint texture;
+  glGenTextures(1, &texture);
+  glBindTexture(GL_TEXTURE_2D, texture);
+
+  // Setup filtering parameters for display
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+
+  // Upload pixels into texture
+  glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0,
+               GLPixelFormat(format), GL_UNSIGNED_BYTE, data);
+
+  return reinterpret_cast<ImTextureID>(static_cast<uintptr_t>(texture));
+}
+
+void gui::UpdateTexture(ImTextureID texture, PixelFormat format, int width,
+                        int height, const unsigned char* data) {
+  GLuint glTexture = static_cast<GLuint>(reinterpret_cast<uintptr_t>(texture));
+  if (glTexture == 0) return;
+  glBindTexture(GL_TEXTURE_2D, glTexture);
+  glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GLPixelFormat(format),
+                  GL_UNSIGNED_BYTE, data);
+}
+
+void gui::DeleteTexture(ImTextureID texture) {
+  if (!gPlatformValid) return;
+  GLuint glTexture = static_cast<GLuint>(reinterpret_cast<uintptr_t>(texture));
+  if (glTexture != 0) glDeleteTextures(1, &glTexture);
+}
+
+}  // namespace wpi

wpilibNewCommands/build.gradle

diff --git a/wpilibNewCommands/build.gradle b/wpilibNewCommands/build.gradle
index 09f2a95..82affa0 100644
--- a/wpilibNewCommands/build.gradle
+++ b/wpilibNewCommands/build.gradle
@@ -6,6 +6,7 @@
 evaluationDependsOn(':ntcore')

 evaluationDependsOn(':cscore')

 evaluationDependsOn(':hal')

+evaluationDependsOn(':wpimath')

 evaluationDependsOn(':wpilibc')

 evaluationDependsOn(':cameraserver')

 evaluationDependsOn(':wpilibj')

@@ -17,11 +18,13 @@
     implementation project(':ntcore')

     implementation project(':cscore')

     implementation project(':hal')

+    implementation project(':wpimath')

     implementation project(':wpilibj')

     devImplementation project(':wpiutil')

     devImplementation project(':ntcore')

     devImplementation project(':cscore')

     devImplementation project(':hal')

+    devImplementation project(':wpimath')

     devImplementation project(':wpilibj')

     testImplementation 'com.google.guava:guava:19.0'

     testImplementation 'org.mockito:mockito-core:2.27.0'

@@ -53,6 +56,7 @@
             lib project: ':ntcore', library: 'ntcore', linkage: 'shared'

             project(':hal').addHalDependency(it, 'shared')

             lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'

+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'

 

             if (it.component.name == "${nativeName}Dev") {

               lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
index 6cca974..ba950c7 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,7 +18,6 @@
  * <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless
  * specified explicitly from the command implementation.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public interface Command {
 
   /**
@@ -37,6 +36,9 @@
    * The action to take when the command ends.  Called when either the command finishes normally,
    * or when it interrupted/canceled.
    *
+   * <p>Do not schedule commands here that share requirements with this command.
+   * Use {@link #andThen(Command...)} instead.
+   *
    * @param interrupted whether the command was interrupted/canceled
    */
   default void end(boolean interrupted) {

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
index ba2c68f..49180a8 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -57,6 +57,18 @@
   }
 
   /**
+  * Decorates this Command with a name.
+  * Is an inline function for #setName(String);
+  *
+  * @param name name
+  * @return the decorated Command
+  */
+  public Command withName(String name) {
+    this.setName(name);
+    return this;
+  }
+
+  /**
    * Gets the subsystem name of this Command.
    *
    * @return Subsystem name

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
index d82e77f..60488d2 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,8 +22,12 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Watchdog;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -34,8 +38,8 @@
  * {@link CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link
  * Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
  */
-@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
-public final class CommandScheduler implements Sendable {
+@SuppressWarnings({"PMD.GodClass", "PMD.TooManyFields"})
+public final class CommandScheduler implements Sendable, AutoCloseable {
   /**
    * The Singleton Instance.
    */
@@ -70,11 +74,6 @@
 
   private boolean m_disabled;
 
-  //NetworkTable entries for use in Sendable impl
-  private NetworkTableEntry m_namesEntry;
-  private NetworkTableEntry m_idsEntry;
-  private NetworkTableEntry m_cancelEntry;
-
   //Lists of user-supplied actions to be executed on scheduling events for every command.
   private final List<Consumer<Command>> m_initActions = new ArrayList<>();
   private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
@@ -87,10 +86,35 @@
   private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
   private final List<Command> m_toCancel = new ArrayList<>();
 
+  private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> { });
 
   CommandScheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
+    LiveWindow.setEnabledListener(() -> {
+      disable();
+      cancelAll();
+    });
+    LiveWindow.setDisabledListener(() -> {
+      enable();
+    });
+  }
+
+  /**
+   * Changes the period of the loop overrun watchdog.  This should be be kept in sync with the
+   * TimedRobot period.
+   *
+   * @param period Period in seconds.
+   */
+  public void setPeriod(double period) {
+    m_watchdog.setTimeout(period);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    LiveWindow.setEnabledListener(null);
+    LiveWindow.setDisabledListener(null);
   }
 
   /**
@@ -117,15 +141,17 @@
    * @param requirements  The command requirements
    */
   private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
-    command.initialize();
     CommandState scheduledCommand = new CommandState(interruptible);
     m_scheduledCommands.put(command, scheduledCommand);
-    for (Consumer<Command> action : m_initActions) {
-      action.accept(command);
-    }
+    command.initialize();
     for (Subsystem requirement : requirements) {
       m_requirements.put(requirement, command);
     }
+    for (Consumer<Command> action : m_initActions) {
+      action.accept(command);
+    }
+
+    m_watchdog.addEpoch(command.getName() + ".initialize()");
   }
 
   /**
@@ -223,16 +249,22 @@
     if (m_disabled) {
       return;
     }
+    m_watchdog.reset();
 
     //Run the periodic method of all registered subsystems.
     for (Subsystem subsystem : m_subsystems.keySet()) {
       subsystem.periodic();
+      if (RobotBase.isSimulation()) {
+        subsystem.simulationPeriodic();
+      }
+      m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()");
     }
 
     //Poll buttons for new commands to add.
     for (Runnable button : m_buttons) {
       button.run();
     }
+    m_watchdog.addEpoch("buttons.run()");
 
     m_inRunLoop = true;
     //Run scheduled commands, remove finished commands.
@@ -247,6 +279,7 @@
         }
         m_requirements.keySet().removeAll(command.getRequirements());
         iterator.remove();
+        m_watchdog.addEpoch(command.getName() + ".end(true)");
         continue;
       }
 
@@ -254,6 +287,7 @@
       for (Consumer<Command> action : m_executeActions) {
         action.accept(command);
       }
+      m_watchdog.addEpoch(command.getName() + ".execute()");
       if (command.isFinished()) {
         command.end(false);
         for (Consumer<Command> action : m_finishActions) {
@@ -262,6 +296,7 @@
         iterator.remove();
 
         m_requirements.keySet().removeAll(command.getRequirements());
+        m_watchdog.addEpoch(command.getName() + ".end(false)");
       }
     }
     m_inRunLoop = false;
@@ -285,6 +320,12 @@
         schedule(subsystemCommand.getValue());
       }
     }
+
+    m_watchdog.disable();
+    if (m_watchdog.isExpired()) {
+      System.out.println("CommandScheduler loop overrun");
+      m_watchdog.printEpochs();
+    }
   }
 
   /**
@@ -345,9 +386,11 @@
   }
 
   /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a canceled command,
-   * not the end method (though the interrupted method may itself call the end method).  Commands
-   * will be canceled even if they are not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call {@link Command#end(boolean)} method
+   * of the canceled command with {@code true},
+   * indicating they were canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as interruptible.
    *
    * @param commands the commands to cancel
    */
@@ -368,6 +411,7 @@
       }
       m_scheduledCommands.remove(command);
       m_requirements.keySet().removeAll(command.getRequirements());
+      m_watchdog.addEpoch(command.getName() + ".end(true)");
     }
   }
 
@@ -375,7 +419,7 @@
    * Cancels all commands that are currently scheduled.
    */
   public void cancelAll() {
-    for (Command command : m_scheduledCommands.keySet()) {
+    for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
       cancel(command);
     }
   }
@@ -473,12 +517,12 @@
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Scheduler");
-    m_namesEntry = builder.getEntry("Names");
-    m_idsEntry = builder.getEntry("Ids");
-    m_cancelEntry = builder.getEntry("Cancel");
+    final NetworkTableEntry namesEntry = builder.getEntry("Names");
+    final NetworkTableEntry idsEntry = builder.getEntry("Ids");
+    final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
     builder.setUpdateTable(() -> {
 
-      if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) {
+      if (namesEntry == null || idsEntry == null || cancelEntry == null) {
         return;
       }
 
@@ -489,21 +533,21 @@
         ids.put((double) command.hashCode(), command);
       }
 
-      double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+      double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
       if (toCancel.length > 0) {
         for (double hash : toCancel) {
           cancel(ids.get(hash));
           ids.remove(hash);
         }
-        m_cancelEntry.setDoubleArray(new double[0]);
+        cancelEntry.setDoubleArray(new double[0]);
       }
 
       List<String> names = new ArrayList<>();
 
       ids.values().forEach(command -> names.add(command.getName()));
 
-      m_namesEntry.setStringArray(names.toArray(new String[0]));
-      m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
+      namesEntry.setStringArray(names.toArray(new String[0]));
+      idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
     });
   }
 }

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
index af50dc8..9105f7a 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,10 +11,12 @@
 import java.util.function.Supplier;
 
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
@@ -45,18 +47,13 @@
 @SuppressWarnings({"PMD.TooManyFields", "MemberName"})
 public class MecanumControllerCommand extends CommandBase {
   private final Timer m_timer = new Timer();
-  private MecanumDriveWheelSpeeds m_prevSpeeds;
-  private double m_prevTime;
-  private Pose2d m_finalPose;
   private final boolean m_usePID;
-
   private final Trajectory m_trajectory;
   private final Supplier<Pose2d> m_pose;
   private final SimpleMotorFeedforward m_feedforward;
   private final MecanumDriveKinematics m_kinematics;
-  private final PIDController m_xController;
-  private final PIDController m_yController;
-  private final ProfiledPIDController m_thetaController;
+  private final HolonomicDriveController m_controller;
+  private final Supplier<Rotation2d> m_desiredRotation;
   private final double m_maxWheelVelocityMetersPerSecond;
   private final PIDController m_frontLeftController;
   private final PIDController m_rearLeftController;
@@ -65,6 +62,8 @@
   private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
   private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
   private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
+  private MecanumDriveWheelSpeeds m_prevSpeeds;
+  private double m_prevTime;
 
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow the provided
@@ -74,8 +73,105 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
    * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The rotation controller will calculate the rotation based on the final pose in the
-   * trajectory, not the poses at each time step.
+   * @param trajectory                      The trajectory to follow.
+   * @param pose                            A function that supplies the robot pose - use one of
+   *                                        the odometry classes to provide this.
+   * @param feedforward                     The feedforward to use for the drivetrain.
+   * @param kinematics                      The kinematics for the robot drivetrain.
+   * @param xController                     The Trajectory Tracker PID controller
+   *                                        for the robot's x position.
+   * @param yController                     The Trajectory Tracker PID controller
+   *                                        for the robot's y position.
+   * @param thetaController                 The Trajectory Tracker PID controller
+   *                                        for angle for the robot.
+   * @param desiredRotation                 The angle that the robot should be facing. This
+   *                                        is sampled at each time step.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController             The front left wheel velocity PID.
+   * @param rearLeftController              The rear left wheel velocity PID.
+   * @param frontRightController            The front right wheel velocity PID.
+   * @param rearRightController             The rear right wheel velocity PID.
+   * @param currentWheelSpeeds              A MecanumDriveWheelSpeeds object containing
+   *                                        the current wheel speeds.
+   * @param outputDriveVoltages             A MecanumDriveMotorVoltages object containing
+   *                                        the output motor voltages.
+   * @param requirements                    The subsystems to require.
+   */
+
+  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+  public MecanumControllerCommand(Trajectory trajectory,
+                                  Supplier<Pose2d> pose,
+                                  SimpleMotorFeedforward feedforward,
+                                  MecanumDriveKinematics kinematics,
+                                  PIDController xController,
+                                  PIDController yController,
+                                  ProfiledPIDController thetaController,
+                                  Supplier<Rotation2d> desiredRotation,
+                                  double maxWheelVelocityMetersPerSecond,
+                                  PIDController frontLeftController,
+                                  PIDController rearLeftController,
+                                  PIDController frontRightController,
+                                  PIDController rearRightController,
+                                  Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+                                  Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+                                  Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory",
+        "MecanumControllerCommand");
+    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
+    m_feedforward = requireNonNullParam(feedforward, "feedforward",
+        "MecanumControllerCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics",
+        "MecanumControllerCommand");
+
+    m_controller = new HolonomicDriveController(
+        requireNonNullParam(xController,
+            "xController", "SwerveControllerCommand"),
+        requireNonNullParam(yController,
+            "xController", "SwerveControllerCommand"),
+        requireNonNullParam(thetaController,
+            "thetaController", "SwerveControllerCommand")
+    );
+
+    m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
+        "MecanumControllerCommand");
+
+    m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
+        "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+
+    m_frontLeftController = requireNonNullParam(frontLeftController,
+        "frontLeftController", "MecanumControllerCommand");
+    m_rearLeftController = requireNonNullParam(rearLeftController,
+        "rearLeftController", "MecanumControllerCommand");
+    m_frontRightController = requireNonNullParam(frontRightController,
+        "frontRightController", "MecanumControllerCommand");
+    m_rearRightController = requireNonNullParam(rearRightController,
+        "rearRightController", "MecanumControllerCommand");
+
+    m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
+        "currentWheelSpeeds", "MecanumControllerCommand");
+
+    m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
+        "outputDriveVoltages", "MecanumControllerCommand");
+
+    m_outputWheelSpeeds = null;
+
+    m_usePID = true;
+
+    addRequirements(requirements);
+  }
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to
+   * 12 as a voltage output to the motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory                      The trajectory to follow.
    * @param pose                            A function that supplies the robot pose - use one of
@@ -102,58 +198,100 @@
 
   @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
   public MecanumControllerCommand(Trajectory trajectory,
-                                Supplier<Pose2d> pose,
-                                SimpleMotorFeedforward feedforward,
-                                MecanumDriveKinematics kinematics,
+                                  Supplier<Pose2d> pose,
+                                  SimpleMotorFeedforward feedforward,
+                                  MecanumDriveKinematics kinematics,
+                                  PIDController xController,
+                                  PIDController yController,
+                                  ProfiledPIDController thetaController,
+                                  double maxWheelVelocityMetersPerSecond,
+                                  PIDController frontLeftController,
+                                  PIDController rearLeftController,
+                                  PIDController frontRightController,
+                                  PIDController rearRightController,
+                                  Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+                                  Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+                                  Subsystem... requirements) {
 
-                                PIDController xController,
-                                PIDController yController,
-                                ProfiledPIDController thetaController,
+    this(trajectory, pose, feedforward, kinematics, xController, yController, thetaController,
+        () -> trajectory.getStates().get(trajectory.getStates().size() - 1)
+            .poseMeters.getRotation(),
+        maxWheelVelocityMetersPerSecond, frontLeftController, rearLeftController,
+        frontRightController, rearRightController, currentWheelSpeeds, outputDriveVoltages,
+        requirements);
+  }
 
-                                double maxWheelVelocityMetersPerSecond,
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow the provided
+   * trajectory. The user should implement a velocity PID on the desired output wheel velocities.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
+   * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
+   *
+   * @param trajectory                      The trajectory to follow.
+   * @param pose                            A function that supplies the robot pose - use one of
+   *                                        the odometry classes to provide this.
+   * @param kinematics                      The kinematics for the robot drivetrain.
+   * @param xController                     The Trajectory Tracker PID controller
+   *                                        for the robot's x position.
+   * @param yController                     The Trajectory Tracker PID controller
+   *                                        for the robot's y position.
+   * @param thetaController                 The Trajectory Tracker PID controller
+   *                                        for angle for the robot.
+   * @param desiredRotation                 The angle that the robot should be facing. This
+   *                                        is sampled at each time step.
+   * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
+   * @param outputWheelSpeeds               A MecanumDriveWheelSpeeds object containing
+   *                                        the output wheel speeds.
+   * @param requirements                    The subsystems to require.
+   */
 
-                                PIDController frontLeftController,
-                                PIDController rearLeftController,
-                                PIDController frontRightController,
-                                PIDController rearRightController,
-
-                                Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
-
-                                Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
-                                Subsystem... requirements) {
-    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
+  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
+  public MecanumControllerCommand(Trajectory trajectory,
+                                  Supplier<Pose2d> pose,
+                                  MecanumDriveKinematics kinematics,
+                                  PIDController xController,
+                                  PIDController yController,
+                                  ProfiledPIDController thetaController,
+                                  Supplier<Rotation2d> desiredRotation,
+                                  double maxWheelVelocityMetersPerSecond,
+                                  Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
+                                  Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory",
+        "MecanumControllerCommand");
     m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
-    m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
-    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
+    m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
+    m_kinematics = requireNonNullParam(kinematics,
+        "kinematics", "MecanumControllerCommand");
 
-    m_xController = requireNonNullParam(xController, "xController",
-      "MecanumControllerCommand");
-    m_yController = requireNonNullParam(yController, "yController",
-      "MecanumControllerCommand");
-    m_thetaController = requireNonNullParam(thetaController, "thetaController",
-      "MecanumControllerCommand");
+    m_controller = new HolonomicDriveController(
+        requireNonNullParam(xController,
+            "xController", "SwerveControllerCommand"),
+        requireNonNullParam(yController,
+            "xController", "SwerveControllerCommand"),
+        requireNonNullParam(thetaController,
+            "thetaController", "SwerveControllerCommand")
+    );
+
+    m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
+        "MecanumControllerCommand");
 
     m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
-      "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+        "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
 
-    m_frontLeftController = requireNonNullParam(frontLeftController,
-      "frontLeftController", "MecanumControllerCommand");
-    m_rearLeftController = requireNonNullParam(rearLeftController,
-      "rearLeftController", "MecanumControllerCommand");
-    m_frontRightController = requireNonNullParam(frontRightController,
-      "frontRightController", "MecanumControllerCommand");
-    m_rearRightController = requireNonNullParam(rearRightController,
-      "rearRightController", "MecanumControllerCommand");
+    m_frontLeftController = null;
+    m_rearLeftController = null;
+    m_frontRightController = null;
+    m_rearRightController = null;
 
-    m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
-      "currentWheelSpeeds", "MecanumControllerCommand");
+    m_currentWheelSpeeds = null;
 
-    m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
-    "outputDriveVoltages", "MecanumControllerCommand");
+    m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
+        "outputWheelSpeeds", "MecanumControllerCommand");
 
-    m_outputWheelSpeeds = null;
+    m_outputDriveVoltages = null;
 
-    m_usePID = true;
+    m_usePID = false;
 
     addRequirements(requirements);
   }
@@ -165,8 +303,10 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
    * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
    *
-   * <p>Note2: The rotation controller will calculate the rotation based on the final pose
-   * in the trajectory, not the poses at each time step.
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory                      The trajectory to follow.
    * @param pose                            A function that supplies the robot pose - use one of
@@ -186,63 +326,31 @@
 
   @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
   public MecanumControllerCommand(Trajectory trajectory,
-                                Supplier<Pose2d> pose,
-                                MecanumDriveKinematics kinematics,
-                                PIDController xController,
-                                PIDController yController,
-                                ProfiledPIDController thetaController,
-
-                                double maxWheelVelocityMetersPerSecond,
-
-                                Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
-                                Subsystem... requirements) {
-    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
-    m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
-    m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
-    m_kinematics = requireNonNullParam(kinematics,
-      "kinematics", "MecanumControllerCommand");
-
-    m_xController = requireNonNullParam(xController,
-      "xController", "MecanumControllerCommand");
-    m_yController = requireNonNullParam(yController,
-      "xController", "MecanumControllerCommand");
-    m_thetaController = requireNonNullParam(thetaController,
-      "thetaController", "MecanumControllerCommand");
-
-    m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
-      "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
-
-    m_frontLeftController = null;
-    m_rearLeftController = null;
-    m_frontRightController = null;
-    m_rearRightController = null;
-
-    m_currentWheelSpeeds = null;
-
-    m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
-      "outputWheelSpeeds", "MecanumControllerCommand");
-
-    m_outputDriveVoltages = null;
-
-    m_usePID = false;
-
-    addRequirements(requirements);
+                                  Supplier<Pose2d> pose,
+                                  MecanumDriveKinematics kinematics,
+                                  PIDController xController,
+                                  PIDController yController,
+                                  ProfiledPIDController thetaController,
+                                  double maxWheelVelocityMetersPerSecond,
+                                  Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
+                                  Subsystem... requirements) {
+    this(trajectory, pose, kinematics, xController, yController, thetaController,
+        () -> trajectory.getStates().get(trajectory.getStates().size() - 1)
+            .poseMeters.getRotation(),
+        maxWheelVelocityMetersPerSecond, outputWheelSpeeds, requirements);
   }
 
   @Override
   public void initialize() {
     var initialState = m_trajectory.sample(0);
 
-    // Sample final pose to get robot rotation
-    m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
-
     var initialXVelocity = initialState.velocityMetersPerSecond
         * initialState.poseMeters.getRotation().getCos();
     var initialYVelocity = initialState.velocityMetersPerSecond
         * initialState.poseMeters.getRotation().getSin();
 
     m_prevSpeeds = m_kinematics.toWheelSpeeds(
-      new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
+        new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
 
     m_timer.reset();
     m_timer.start();
@@ -255,31 +363,9 @@
     double dt = curTime - m_prevTime;
 
     var desiredState = m_trajectory.sample(curTime);
-    var desiredPose = desiredState.poseMeters;
 
-    var poseError = desiredPose.relativeTo(m_pose.get());
-
-    double targetXVel = m_xController.calculate(
-        m_pose.get().getTranslation().getX(),
-        desiredPose.getTranslation().getX());
-
-    double targetYVel = m_yController.calculate(
-        m_pose.get().getTranslation().getY(),
-        desiredPose.getTranslation().getY());
-
-    // The robot will go to the desired rotation of the final pose in the trajectory,
-    // not following the poses at individual states.
-    double targetAngularVel = m_thetaController.calculate(
-        m_pose.get().getRotation().getRadians(),
-        m_finalPose.getRotation().getRadians());
-
-    double vRef = desiredState.velocityMetersPerSecond;
-
-    targetXVel += vRef * poseError.getRotation().getCos();
-    targetYVel += vRef * poseError.getRotation().getSin();
-
-    var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
-
+    var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
+        m_desiredRotation.get());
     var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
 
     targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
@@ -287,7 +373,7 @@
     var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
     var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
     var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
-    var rearRightSpeedSetpoint =  targetWheelSpeeds.rearRightMetersPerSecond;
+    var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
 
     double frontLeftOutput;
     double rearLeftOutput;
@@ -296,32 +382,32 @@
 
     if (m_usePID) {
       final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint,
-              (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
+          (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
 
       final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint,
-              (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
+          (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
 
       final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint,
-              (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
+          (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
 
       final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint,
-            (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
+          (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
 
       frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(
-            m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
-             frontLeftSpeedSetpoint);
+          m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
+          frontLeftSpeedSetpoint);
 
       rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(
-            m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
-             rearLeftSpeedSetpoint);
+          m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
+          rearLeftSpeedSetpoint);
 
       frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(
-            m_currentWheelSpeeds.get().frontRightMetersPerSecond,
-             frontRightSpeedSetpoint);
+          m_currentWheelSpeeds.get().frontRightMetersPerSecond,
+          frontRightSpeedSetpoint);
 
       rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(
-            m_currentWheelSpeeds.get().rearRightMetersPerSecond,
-             rearRightSpeedSetpoint);
+          m_currentWheelSpeeds.get().rearRightMetersPerSecond,
+          rearRightSpeedSetpoint);
 
       m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(
           frontLeftOutput,
@@ -348,6 +434,6 @@
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
 }

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
index f9337c6..4cc578d 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -30,6 +30,7 @@
   public PIDSubsystem(PIDController controller, double initialPosition) {
     setSetpoint(initialPosition);
     m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
+    addChild("PID Controller", m_controller);
   }
 
   /**
@@ -62,6 +63,15 @@
   }
 
   /**
+   * Returns the current setpoint of the subsystem.
+   *
+   * @return The current setpoint
+   */
+  public double getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
    * Uses the output from the PIDController.
    *
    * @param output the output of the PIDController

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
index eea06c7..c6a2b4c 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -201,6 +201,6 @@
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
 }

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
index 3139465..9029135 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 /**
- * A command that runs a given runnable when it is initalized, and another runnable when it ends.
+ * A command that runs a given runnable when it is initialized, and another runnable when it ends.
  * Useful for running and then stopping a motor, or extending and then retracting a solenoid.
  * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
  * {@link Command#withInterrupt(BooleanSupplier)} to give it one.

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
index 7dc917b..e693d87 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -34,6 +34,14 @@
   }
 
   /**
+   * This method is called periodically by the {@link CommandScheduler}.  Useful for updating
+   * subsystem-specific state that needs to be maintained for simulations, such as for updating
+   * {@link edu.wpi.first.wpilibj.simulation} classes and setting simulated sensor readings.
+   */
+  default void simulationPeriodic() {
+  }
+
+  /**
    * Sets the default {@link Command} of the subsystem.  The default command will be
    * automatically scheduled when no other commands are scheduled that require the subsystem.
    * Default commands should generally not end on their own, i.e. their {@link Command#isFinished()}

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
index b05dc20..c0303b9 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,15 +7,15 @@
 
 package edu.wpi.first.wpilibj2.command;
 
-
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
@@ -38,15 +38,12 @@
 @SuppressWarnings("MemberName")
 public class SwerveControllerCommand extends CommandBase {
   private final Timer m_timer = new Timer();
-  private Pose2d m_finalPose;
-
   private final Trajectory m_trajectory;
   private final Supplier<Pose2d> m_pose;
   private final SwerveDriveKinematics m_kinematics;
-  private final PIDController m_xController;
-  private final PIDController m_yController;
-  private final ProfiledPIDController m_thetaController;
+  private final HolonomicDriveController m_controller;
   private final Consumer<SwerveModuleState[]> m_outputModuleStates;
+  private final Supplier<Rotation2d> m_desiredRotation;
 
   /**
    * Constructs a new SwerveControllerCommand that when executed will follow the provided
@@ -56,8 +53,66 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
    * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The rotation controller will calculate the rotation based on the final pose
-   * in the trajectory, not the poses at each time step.
+   * @param trajectory         The trajectory to follow.
+   * @param pose               A function that supplies the robot pose - use one of
+   *                           the odometry classes to provide this.
+   * @param kinematics         The kinematics for the robot drivetrain.
+   * @param xController        The Trajectory Tracker PID controller
+   *                           for the robot's x position.
+   * @param yController        The Trajectory Tracker PID controller
+   *                           for the robot's y position.
+   * @param thetaController    The Trajectory Tracker PID controller
+   *                           for angle for the robot.
+   * @param desiredRotation    The angle that the drivetrain should be facing. This
+   *                           is sampled at each time step.
+   * @param outputModuleStates The raw output module states from the
+   *                           position controllers.
+   * @param requirements       The subsystems to require.
+   */
+  @SuppressWarnings("ParameterName")
+  public SwerveControllerCommand(Trajectory trajectory,
+                                 Supplier<Pose2d> pose,
+                                 SwerveDriveKinematics kinematics,
+                                 PIDController xController,
+                                 PIDController yController,
+                                 ProfiledPIDController thetaController,
+                                 Supplier<Rotation2d> desiredRotation,
+                                 Consumer<SwerveModuleState[]> outputModuleStates,
+                                 Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
+    m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
+
+    m_controller = new HolonomicDriveController(
+        requireNonNullParam(xController,
+            "xController", "SwerveControllerCommand"),
+        requireNonNullParam(yController,
+            "xController", "SwerveControllerCommand"),
+        requireNonNullParam(thetaController,
+            "thetaController", "SwerveControllerCommand")
+    );
+
+    m_outputModuleStates = requireNonNullParam(outputModuleStates,
+        "frontLeftOutput", "SwerveControllerCommand");
+
+    m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
+        "SwerveControllerCommand");
+
+    addRequirements(requirements);
+  }
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the provided
+   * trajectory. This command will not return output voltages but rather raw module states from the
+   * position controllers which need to be put into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory         The trajectory to follow.
    * @param pose               A function that supplies the robot pose - use one of
@@ -73,38 +128,23 @@
    *                           position controllers.
    * @param requirements       The subsystems to require.
    */
-
   @SuppressWarnings("ParameterName")
   public SwerveControllerCommand(Trajectory trajectory,
-                               Supplier<Pose2d> pose,
-                               SwerveDriveKinematics kinematics,
-                               PIDController xController,
-                               PIDController yController,
-                               ProfiledPIDController thetaController,
-
-                               Consumer<SwerveModuleState[]> outputModuleStates,
-                               Subsystem... requirements) {
-    m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
-    m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
-    m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
-
-    m_xController = requireNonNullParam(xController,
-      "xController", "SwerveControllerCommand");
-    m_yController = requireNonNullParam(yController,
-      "xController", "SwerveControllerCommand");
-    m_thetaController = requireNonNullParam(thetaController,
-      "thetaController", "SwerveControllerCommand");
-
-    m_outputModuleStates = requireNonNullParam(outputModuleStates,
-      "frontLeftOutput", "SwerveControllerCommand");
-    addRequirements(requirements);
+                                 Supplier<Pose2d> pose,
+                                 SwerveDriveKinematics kinematics,
+                                 PIDController xController,
+                                 PIDController yController,
+                                 ProfiledPIDController thetaController,
+                                 Consumer<SwerveModuleState[]> outputModuleStates,
+                                 Subsystem... requirements) {
+    this(trajectory, pose, kinematics, xController, yController, thetaController,
+        () -> trajectory.getStates().get(trajectory.getStates().size() - 1)
+            .poseMeters.getRotation(),
+        outputModuleStates, requirements);
   }
 
   @Override
   public void initialize() {
-    // Sample final pose to get robot rotation
-    m_finalPose = m_trajectory.sample(m_trajectory.getTotalTimeSeconds()).poseMeters;
-
     m_timer.reset();
     m_timer.start();
   }
@@ -113,37 +153,13 @@
   @SuppressWarnings("LocalVariableName")
   public void execute() {
     double curTime = m_timer.get();
-
     var desiredState = m_trajectory.sample(curTime);
-    var desiredPose = desiredState.poseMeters;
 
-    var poseError = desiredPose.relativeTo(m_pose.get());
-
-    double targetXVel = m_xController.calculate(
-        m_pose.get().getTranslation().getX(),
-        desiredPose.getTranslation().getX());
-
-    double targetYVel = m_yController.calculate(
-        m_pose.get().getTranslation().getY(),
-        desiredPose.getTranslation().getY());
-
-    // The robot will go to the desired rotation of the final pose in the trajectory,
-    // not following the poses at individual states.
-    double targetAngularVel = m_thetaController.calculate(
-        m_pose.get().getRotation().getRadians(),
-        m_finalPose.getRotation().getRadians());
-
-    double vRef = desiredState.velocityMetersPerSecond;
-
-    targetXVel += vRef * poseError.getRotation().getCos();
-    targetYVel += vRef * poseError.getRotation().getSin();
-
-    var targetChassisSpeeds = new ChassisSpeeds(targetXVel, targetYVel, targetAngularVel);
-
+    var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
+        m_desiredRotation.get());
     var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
 
     m_outputModuleStates.accept(targetModuleStates);
-
   }
 
   @Override
@@ -153,6 +169,6 @@
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
   }
 }

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
index 85ddd03..0e645c6 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -59,6 +59,6 @@
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_profile.totalTime());
+    return m_timer.hasElapsed(m_profile.totalTime());
   }
 }

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
index 5e8ebc3..c53fb42 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -41,7 +41,7 @@
 
   @Override
   public boolean isFinished() {
-    return m_timer.hasPeriodPassed(m_duration);
+    return m_timer.hasElapsed(m_duration);
   }
 
   @Override

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
index b7f26ae..d672a38 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,8 +22,7 @@
  * operator interface as a common use case of the more generalized Trigger objects. This is a simple
  * wrapper around Trigger with the method names renamed to fit the Button object use.
  */
-@SuppressWarnings("PMD.TooManyMethods")
-public abstract class Button extends Trigger {
+public class Button extends Trigger {
   /**
    * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
    * overridden).

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java
new file mode 100644
index 0000000..4cb1859
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/**
+ * A {@link Button} that uses a {@link NetworkTable} boolean field.
+ */
+public class NetworkButton extends Button {
+  private final NetworkTableEntry m_entry;
+
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param entry The entry that is the value.
+   */
+  public NetworkButton(NetworkTableEntry entry) {
+    m_entry = entry;
+  }
+
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param table The table where the networktable value is located.
+   * @param field The field that is the value.
+   */
+  public NetworkButton(NetworkTable table, String field) {
+    this(table.getEntry(field));
+  }
+
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param table The table where the networktable value is located.
+   * @param field The field that is the value.
+   */
+  public NetworkButton(String table, String field) {
+    this(NetworkTableInstance.getDefault().getTable(table), field);
+  }
+
+  @Override
+  public boolean get() {
+    return m_entry.getInstance().isConnected() && m_entry.getBoolean(false);
+  }
+}

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

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
index 5167e4a..e5e1763 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -27,7 +27,6 @@
  * reading a certain sensor input). For this, they only have to write the {@link Trigger#get()}
  * method to get the full functionality of the Trigger class.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class Trigger {
   private final BooleanSupplier m_isActive;
 

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
index f2cfa95..30c3af6 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -13,7 +13,7 @@
 using namespace frc2;
 
 CommandBase::CommandBase() {
-  frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+  frc::SendableRegistry::GetInstance().Add(this, GetTypeName(*this));
 }
 
 void CommandBase::AddRequirements(
@@ -51,14 +51,16 @@
 
 void CommandBase::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Command");
-  builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
-  builder.AddBooleanProperty("running", [this] { return IsScheduled(); },
-                             [this](bool value) {
-                               bool isScheduled = IsScheduled();
-                               if (value && !isScheduled) {
-                                 Schedule();
-                               } else if (!value && isScheduled) {
-                                 Cancel();
-                               }
-                             });
+  builder.AddStringProperty(
+      ".name", [this] { return GetName(); }, nullptr);
+  builder.AddBooleanProperty(
+      "running", [this] { return IsScheduled(); },
+      [this](bool value) {
+        bool isScheduled = IsScheduled();
+        if (value && !isScheduled) {
+          Schedule();
+        } else if (!value && isScheduled) {
+          Cancel();
+        }
+      });
 }

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index 842facb..a59c68b 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -7,14 +7,19 @@
 
 #include "frc2/command/CommandScheduler.h"
 
+#include <frc/RobotBase.h>
 #include <frc/RobotState.h>
+#include <frc/TimedRobot.h>
 #include <frc/WPIErrors.h>
+#include <frc/livewindow/LiveWindow.h>
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <networktables/NetworkTableEntry.h>
 #include <wpi/DenseMap.h>
+#include <wpi/SmallVector.h>
+#include <wpi/raw_ostream.h>
 
 #include "frc2/command/CommandGroupBase.h"
 #include "frc2/command/CommandState.h"
@@ -42,11 +47,6 @@
 
   bool disabled{false};
 
-  // NetworkTable entries for use in Sendable impl
-  nt::NetworkTableEntry namesEntry;
-  nt::NetworkTableEntry idsEntry;
-  nt::NetworkTableEntry cancelEntry;
-
   // Lists of user-supplied actions to be executed on scheduling events for
   // every command.
   wpi::SmallVector<Action, 4> initActions;
@@ -67,19 +67,37 @@
   return map.find(keyToCheck) != map.end();
 }
 
-CommandScheduler::CommandScheduler() : m_impl(new Impl) {
+CommandScheduler::CommandScheduler()
+    : m_impl(new Impl), m_watchdog(frc::TimedRobot::kDefaultPeriod, [] {
+        wpi::outs() << "CommandScheduler loop time overrun.\n";
+      }) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand2_Scheduler);
   frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = [this] {
+    this->Disable();
+    this->CancelAll();
+  };
+  scheduler->disabled = [this] { this->Enable(); };
 }
 
-CommandScheduler::~CommandScheduler() {}
+CommandScheduler::~CommandScheduler() {
+  frc::SendableRegistry::GetInstance().Remove(this);
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = nullptr;
+  scheduler->disabled = nullptr;
+}
 
 CommandScheduler& CommandScheduler::GetInstance() {
   static CommandScheduler scheduler;
   return scheduler;
 }
 
+void CommandScheduler::SetPeriod(units::second_t period) {
+  m_watchdog.SetTimeout(period);
+}
+
 void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
   m_impl->buttons.emplace_back(std::move(button));
 }
@@ -127,12 +145,13 @@
     }
     command->Initialize();
     m_impl->scheduledCommands[command] = CommandState{interruptible};
-    for (auto&& action : m_impl->initActions) {
-      action(*command);
-    }
     for (auto&& requirement : requirements) {
       m_impl->requirements[requirement] = command;
     }
+    for (auto&& action : m_impl->initActions) {
+      action(*command);
+    }
+    m_watchdog.AddEpoch(command->GetName() + ".Initialize()");
   }
 }
 
@@ -169,15 +188,22 @@
     return;
   }
 
+  m_watchdog.Reset();
+
   // Run the periodic method of all registered subsystems.
   for (auto&& subsystem : m_impl->subsystems) {
     subsystem.getFirst()->Periodic();
+    if constexpr (frc::RobotBase::IsSimulation()) {
+      subsystem.getFirst()->SimulationPeriodic();
+    }
+    m_watchdog.AddEpoch("Subsystem Periodic()");
   }
 
   // Poll buttons for new commands to add.
   for (auto&& button : m_impl->buttons) {
     button();
   }
+  m_watchdog.AddEpoch("buttons.Run()");
 
   m_impl->inRunLoop = true;
   // Run scheduled commands, remove finished commands.
@@ -194,6 +220,7 @@
     for (auto&& action : m_impl->executeActions) {
       action(*command);
     }
+    m_watchdog.AddEpoch(command->GetName() + ".Execute()");
 
     if (command->IsFinished()) {
       command->End(false);
@@ -206,6 +233,7 @@
       }
 
       m_impl->scheduledCommands.erase(iterator);
+      m_watchdog.AddEpoch(command->GetName() + ".End(false)");
     }
   }
   m_impl->inRunLoop = false;
@@ -228,6 +256,11 @@
       Schedule({subsystem.getSecond().get()});
     }
   }
+
+  m_watchdog.Disable();
+  if (m_watchdog.IsExpired()) {
+    m_watchdog.PrintEpochs();
+  }
 }
 
 void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
@@ -289,6 +322,7 @@
   for (auto&& action : m_impl->interruptActions) {
     action(*command);
   }
+  m_watchdog.AddEpoch(command->GetName() + ".End(true)");
   m_impl->scheduledCommands.erase(find);
   for (auto&& requirement : m_impl->requirements) {
     if (requirement.second == command) {
@@ -310,9 +344,11 @@
 }
 
 void CommandScheduler::CancelAll() {
+  wpi::SmallVector<Command*, 16> commands;
   for (auto&& command : m_impl->scheduledCommands) {
-    Cancel(command.first);
+    commands.emplace_back(command.first);
   }
+  Cancel(commands);
 }
 
 double CommandScheduler::TimeSinceScheduled(const Command* command) const {
@@ -379,14 +415,14 @@
 
 void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
-  m_impl->namesEntry = builder.GetEntry("Names");
-  m_impl->idsEntry = builder.GetEntry("Ids");
-  m_impl->cancelEntry = builder.GetEntry("Cancel");
+  auto namesEntry = builder.GetEntry("Names");
+  auto idsEntry = builder.GetEntry("Ids");
+  auto cancelEntry = builder.GetEntry("Cancel");
 
-  builder.SetUpdateTable([this] {
+  builder.SetUpdateTable([=] {
     double tmp[1];
     tmp[0] = 0;
-    auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
+    auto toCancel = cancelEntry.GetDoubleArray(tmp);
     for (auto cancel : toCancel) {
       uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
       Command* command = reinterpret_cast<Command*>(ptrTmp);
@@ -394,7 +430,8 @@
           m_impl->scheduledCommands.end()) {
         Cancel(command);
       }
-      m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+      nt::NetworkTableEntry(cancelEntry)
+          .SetDoubleArray(wpi::ArrayRef<double>{});
     }
 
     wpi::SmallVector<std::string, 8> names;
@@ -404,8 +441,8 @@
       uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
       ids.emplace_back(static_cast<double>(ptrTmp));
     }
-    m_impl->namesEntry.SetStringArray(names);
-    m_impl->idsEntry.SetDoubleArray(ids);
+    nt::NetworkTableEntry(namesEntry).SetStringArray(names);
+    nt::NetworkTableEntry(idsEntry).SetDoubleArray(ids);
   });
 }
 

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
index 8809803..eb32853 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -16,6 +16,7 @@
     frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
     frc2::PIDController yController,
     frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<frc::Rotation2d()> desiredRotation,
     units::meters_per_second_t maxWheelVelocity,
     std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
     frc2::PIDController frontLeftController,
@@ -30,11 +31,85 @@
       m_pose(pose),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
-      m_xController(std::make_unique<frc2::PIDController>(xController)),
-      m_yController(std::make_unique<frc2::PIDController>(yController)),
-      m_thetaController(
-          std::make_unique<frc::ProfiledPIDController<units::radians>>(
-              thetaController)),
+      m_controller(xController, yController, thetaController),
+      m_desiredRotation(desiredRotation),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_frontLeftController(
+          std::make_unique<frc2::PIDController>(frontLeftController)),
+      m_rearLeftController(
+          std::make_unique<frc2::PIDController>(rearLeftController)),
+      m_frontRightController(
+          std::make_unique<frc2::PIDController>(frontRightController)),
+      m_rearRightController(
+          std::make_unique<frc2::PIDController>(rearRightController)),
+      m_currentWheelSpeeds(currentWheelSpeeds),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+    frc2::PIDController frontLeftController,
+    frc2::PIDController rearLeftController,
+    frc2::PIDController frontRightController,
+    frc2::PIDController rearRightController,
+    std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                       units::volt_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_controller(xController, yController, thetaController),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_frontLeftController(
+          std::make_unique<frc2::PIDController>(frontLeftController)),
+      m_rearLeftController(
+          std::make_unique<frc2::PIDController>(rearLeftController)),
+      m_frontRightController(
+          std::make_unique<frc2::PIDController>(frontRightController)),
+      m_rearRightController(
+          std::make_unique<frc2::PIDController>(rearRightController)),
+      m_currentWheelSpeeds(currentWheelSpeeds),
+      m_outputVolts(output),
+      m_usePID(true) {
+  AddRequirements(requirements);
+  m_desiredRotation = [&] {
+    return m_trajectory.States().back().pose.Rotation();
+  };
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SimpleMotorFeedforward<units::meters> feedforward,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<frc::Rotation2d()> desiredRotation,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+    frc2::PIDController frontLeftController,
+    frc2::PIDController rearLeftController,
+    frc2::PIDController frontRightController,
+    frc2::PIDController rearRightController,
+    std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                       units::volt_t)>
+        output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_controller(xController, yController, thetaController),
+      m_desiredRotation(desiredRotation),
       m_maxWheelVelocity(maxWheelVelocity),
       m_frontLeftController(
           std::make_unique<frc2::PIDController>(frontLeftController)),
@@ -70,11 +145,7 @@
       m_pose(pose),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
-      m_xController(std::make_unique<frc2::PIDController>(xController)),
-      m_yController(std::make_unique<frc2::PIDController>(yController)),
-      m_thetaController(
-          std::make_unique<frc::ProfiledPIDController<units::radians>>(
-              thetaController)),
+      m_controller(xController, yController, thetaController),
       m_maxWheelVelocity(maxWheelVelocity),
       m_frontLeftController(
           std::make_unique<frc2::PIDController>(frontLeftController)),
@@ -88,6 +159,31 @@
       m_outputVolts(output),
       m_usePID(true) {
   AddRequirements(requirements);
+  m_desiredRotation = [&] {
+    return m_trajectory.States().back().pose.Rotation();
+  };
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<frc::Rotation2d()> desiredRotation,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                       units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_controller(xController, yController, thetaController),
+      m_desiredRotation(desiredRotation),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
 }
 
 MecanumControllerCommand::MecanumControllerCommand(
@@ -103,11 +199,32 @@
     : m_trajectory(trajectory),
       m_pose(pose),
       m_kinematics(kinematics),
-      m_xController(std::make_unique<frc2::PIDController>(xController)),
-      m_yController(std::make_unique<frc2::PIDController>(yController)),
-      m_thetaController(
-          std::make_unique<frc::ProfiledPIDController<units::radians>>(
-              thetaController)),
+      m_controller(xController, yController, thetaController),
+      m_maxWheelVelocity(maxWheelVelocity),
+      m_outputVel(output),
+      m_usePID(false) {
+  AddRequirements(requirements);
+  m_desiredRotation = [&] {
+    return m_trajectory.States().back().pose.Rotation();
+  };
+}
+
+MecanumControllerCommand::MecanumControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+    frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<frc::Rotation2d()> desiredRotation,
+    units::meters_per_second_t maxWheelVelocity,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                       units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_controller(xController, yController, thetaController),
+      m_desiredRotation(desiredRotation),
       m_maxWheelVelocity(maxWheelVelocity),
       m_outputVel(output),
       m_usePID(false) {
@@ -127,15 +244,14 @@
     : m_trajectory(trajectory),
       m_pose(pose),
       m_kinematics(kinematics),
-      m_xController(std::make_unique<frc2::PIDController>(xController)),
-      m_yController(std::make_unique<frc2::PIDController>(yController)),
-      m_thetaController(
-          std::make_unique<frc::ProfiledPIDController<units::radians>>(
-              thetaController)),
+      m_controller(xController, yController, thetaController),
       m_maxWheelVelocity(maxWheelVelocity),
       m_outputVel(output),
       m_usePID(false) {
   AddRequirements(requirements);
+  m_desiredRotation = [&] {
+    return m_trajectory.States().back().pose.Rotation();
+  };
 }
 
 void MecanumControllerCommand::Initialize() {
@@ -165,31 +281,9 @@
   auto dt = curTime - m_prevTime;
 
   auto m_desiredState = m_trajectory.Sample(curTime);
-  auto m_desiredPose = m_desiredState.pose;
-
-  auto m_poseError = m_desiredPose.RelativeTo(m_pose());
-
-  auto targetXVel = meters_per_second_t(
-      m_xController->Calculate((m_pose().Translation().X().to<double>()),
-                               (m_desiredPose.Translation().X().to<double>())));
-  auto targetYVel = meters_per_second_t(
-      m_yController->Calculate((m_pose().Translation().Y().to<double>()),
-                               (m_desiredPose.Translation().Y().to<double>())));
-
-  // Profiled PID Controller only takes meters as setpoint and measurement
-  // The robot will go to the desired rotation of the final pose in the
-  // trajectory, not following the poses at individual states.
-  auto targetAngularVel = radians_per_second_t(m_thetaController->Calculate(
-      m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
-
-  auto vRef = m_desiredState.velocity;
-
-  targetXVel += vRef * m_poseError.Rotation().Cos();
-  targetYVel += vRef * m_poseError.Rotation().Sin();
 
   auto targetChassisSpeeds =
-      frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
-
+      m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
   auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
 
   targetWheelSpeeds.Normalize(m_maxWheelVelocity);
@@ -247,5 +341,5 @@
 void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); }
 
 bool MecanumControllerCommand::IsFinished() {
-  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+  return m_timer.HasElapsed(m_trajectory.TotalTime());
 }

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
index 8ce3c37..ea6c105 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
@@ -37,15 +37,17 @@
                        std::function<double()> measurementSource,
                        double setpoint, std::function<void(double)> useOutput,
                        std::initializer_list<Subsystem*> requirements)
-    : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
-                 useOutput, requirements) {}
+    : PIDCommand(
+          controller, measurementSource, [setpoint] { return setpoint; },
+          useOutput, requirements) {}
 
 PIDCommand::PIDCommand(PIDController controller,
                        std::function<double()> measurementSource,
                        double setpoint, std::function<void(double)> useOutput,
                        wpi::ArrayRef<Subsystem*> requirements)
-    : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
-                 useOutput, requirements) {}
+    : PIDCommand(
+          controller, measurementSource, [setpoint] { return setpoint; },
+          useOutput, requirements) {}
 
 void PIDCommand::Initialize() { m_controller.Reset(); }
 

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
index 14d965c..f7d988d 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
     : m_controller{controller} {
   SetSetpoint(initialPosition);
+  AddChild("PID Controller", &m_controller);
 }
 
 void PIDSubsystem::Periodic() {
@@ -22,6 +23,8 @@
 
 void PIDSubsystem::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
 
+double PIDSubsystem::GetSetpoint() const { return m_setpoint; }
+
 void PIDSubsystem::Enable() {
   m_controller.Reset();
   m_enabled = true;

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
index 466cc79..5fc5e31 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,7 @@
 }
 
 void ParallelRaceGroup::Initialize() {
+  m_finished = false;
   for (auto& commandRunning : m_commands) {
     commandRunning->Initialize();
   }

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
index b1608bf..e2c56ab 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -138,5 +138,5 @@
 void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
 
 bool RamseteCommand::IsFinished() {
-  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+  return m_timer.HasElapsed(m_trajectory.TotalTime());
 }

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
index cccb55b..010fcb1 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,6 +14,8 @@
 
 void Subsystem::Periodic() {}
 
+void Subsystem::SimulationPeriodic() {}
+
 Command* Subsystem::GetDefaultCommand() const {
   return CommandScheduler::GetInstance().GetDefaultCommand(this);
 }

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
index 9b30fec..a28de20 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,26 +22,28 @@
 
 void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Subsystem");
-  builder.AddBooleanProperty(".hasDefault",
-                             [this] { return GetDefaultCommand() != nullptr; },
-                             nullptr);
-  builder.AddStringProperty(".default",
-                            [this]() -> std::string {
-                              auto command = GetDefaultCommand();
-                              if (command == nullptr) return "none";
-                              return command->GetName();
-                            },
-                            nullptr);
-  builder.AddBooleanProperty(".hasCommand",
-                             [this] { return GetCurrentCommand() != nullptr; },
-                             nullptr);
-  builder.AddStringProperty(".command",
-                            [this]() -> std::string {
-                              auto command = GetCurrentCommand();
-                              if (command == nullptr) return "none";
-                              return command->GetName();
-                            },
-                            nullptr);
+  builder.AddBooleanProperty(
+      ".hasDefault", [this] { return GetDefaultCommand() != nullptr; },
+      nullptr);
+  builder.AddStringProperty(
+      ".default",
+      [this]() -> std::string {
+        auto command = GetDefaultCommand();
+        if (command == nullptr) return "none";
+        return command->GetName();
+      },
+      nullptr);
+  builder.AddBooleanProperty(
+      ".hasCommand", [this] { return GetCurrentCommand() != nullptr; },
+      nullptr);
+  builder.AddStringProperty(
+      ".command",
+      [this]() -> std::string {
+        auto command = GetCurrentCommand();
+        if (command == nullptr) return "none";
+        return command->GetName();
+      },
+      nullptr);
 }
 
 std::string SubsystemBase::GetName() const {

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
index 1279d66..5c49bee 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,6 @@
 
 void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
 
-bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); }
+bool WaitCommand::IsFinished() { return m_timer.HasElapsed(m_duration); }
 
 bool WaitCommand::RunsWhenDisabled() const { return true; }

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/NetworkButton.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/NetworkButton.cpp
new file mode 100644
index 0000000..01214e0
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/NetworkButton.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/NetworkButton.h"
+
+using namespace frc2;
+
+NetworkButton::NetworkButton(nt::NetworkTableEntry entry)
+    : Button([entry] {
+        return entry.GetInstance().IsConnected() && entry.GetBoolean(false);
+      }) {}
+
+NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+                             const wpi::Twine& field)
+    : NetworkButton(table->GetEntry(field)) {}
+
+NetworkButton::NetworkButton(const wpi::Twine& table, const wpi::Twine& field)
+    : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(table),
+                    field) {}

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

diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
index 5c1a8f7..875b54e 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -15,8 +15,8 @@
 
 Trigger Trigger::WhenActive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Schedule(interruptible);
@@ -41,8 +41,8 @@
 
 Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (pressed) {
           command->Schedule(interruptible);
@@ -70,8 +70,8 @@
 
 Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Schedule(interruptible);
@@ -86,8 +86,8 @@
 
 Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (pressedLast && !pressed) {
           command->Schedule(interruptible);
@@ -111,8 +111,8 @@
 
 Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           if (command->IsScheduled()) {
@@ -129,8 +129,8 @@
 
 Trigger Trigger::CancelWhenActive(Command* command) {
   CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command]() mutable {
-        bool pressed = Get();
+      [pressedLast = m_isActive(), *this, command]() mutable {
+        bool pressed = m_isActive();
 
         if (!pressedLast && pressed) {
           command->Cancel();

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index e50f09e..060832c 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -13,7 +13,7 @@
 #include <string>
 
 #include <frc/ErrorBase.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/Demangle.h>
 #include <wpi/SmallSet.h>

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index f1d0917..4711695 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -13,8 +13,10 @@
 
 #include <frc/ErrorBase.h>
 #include <frc/WPIErrors.h>
+#include <frc/Watchdog.h>
 #include <frc/smartdashboard/Sendable.h>
 #include <frc/smartdashboard/SendableHelper.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/FunctionExtras.h>
 
@@ -47,6 +49,12 @@
   using Action = std::function<void(const Command&)>;
 
   /**
+   * Changes the period of the loop overrun watchdog. This should be kept in
+   * sync with the TimedRobot period.
+   */
+  void SetPeriod(units::second_t period);
+
+  /**
    * Adds a button binding to the scheduler, which will be polled to schedule
    * commands.
    *
@@ -201,30 +209,36 @@
   Command* GetDefaultCommand(const Subsystem* subsystem) const;
 
   /**
-   * Cancels a command.  The scheduler will only call the interrupted method of
-   * a canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
    *
-   * @param command the command to cancel
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
+   *
+   * @param commands the commands to cancel
    */
   void Cancel(Command* command);
 
   /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a
-   * canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
    *
    * @param commands the commands to cancel
    */
   void Cancel(wpi::ArrayRef<Command*> commands);
 
   /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a
-   * canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
+   * Cancels commands. The scheduler will only call Command::End()
+   * method of the canceled command with true, indicating they were
+   * canceled (as opposed to finishing normally).
+   *
+   * <p>Commands will be canceled even if they are not scheduled as
+   * interruptible.
    *
    * @param commands the commands to cancel
    */
@@ -337,6 +351,8 @@
   class Impl;
   std::unique_ptr<Impl> m_impl;
 
+  frc::Watchdog m_watchdog;
+
   friend class CommandTestBase;
 };
 }  // namespace frc2

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
index f667e5f..87b16ee 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
@@ -10,6 +10,7 @@
 #include <initializer_list>
 #include <memory>
 
+#include <frc/controller/HolonomicDriveController.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
@@ -18,7 +19,10 @@
 #include <frc/kinematics/MecanumDriveKinematics.h>
 #include <frc/kinematics/MecanumDriveWheelSpeeds.h>
 #include <frc/trajectory/Trajectory.h>
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/ArrayRef.h>
 
 #include "CommandBase.h"
@@ -60,8 +64,61 @@
    * completion of the path this is left to the user, since it is not
    * appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The rotation controller will calculate the rotation based on the
-   * final pose in the trajectory, not the poses at each time step.
+   * @param trajectory           The trajectory to follow.
+   * @param pose                 A function that supplies the robot pose,
+   *                             provided by the odometry class.
+   * @param feedforward          The feedforward to use for the drivetrain.
+   * @param kinematics           The kinematics for the robot drivetrain.
+   * @param xController          The Trajectory Tracker PID controller
+   *                             for the robot's x position.
+   * @param yController          The Trajectory Tracker PID controller
+   *                             for the robot's y position.
+   * @param thetaController      The Trajectory Tracker PID controller
+   *                             for angle for the robot.
+   * @param desiredRotation      The angle that the robot should be facing.
+   *                             This is sampled at each time step.
+   * @param maxWheelVelocity     The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController  The front left wheel velocity PID.
+   * @param rearLeftController   The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController  The rear right wheel velocity PID.
+   * @param currentWheelSpeeds   A MecanumDriveWheelSpeeds object containing
+   *                             the current wheel speeds.
+   * @param output               The output of the velocity PIDs.
+   * @param requirements         The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SimpleMotorFeedforward<units::meters> feedforward,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<frc::Rotation2d()> desiredRotation,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+      frc2::PIDController frontLeftController,
+      frc2::PIDController rearLeftController,
+      frc2::PIDController frontRightController,
+      frc2::PIDController rearRightController,
+      std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                         units::volt_t)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. PID control and feedforward are handled
+   * internally. Outputs are scaled from -12 to 12 as a voltage output to the
+   * motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory           The trajectory to follow.
    * @param pose                 A function that supplies the robot pose,
@@ -111,8 +168,61 @@
    * completion of the path this is left to the user, since it is not
    * appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The rotation controller will calculate the rotation based on the
-   * final pose in the trajectory, not the poses at each time step.
+   * @param trajectory           The trajectory to follow.
+   * @param pose                 A function that supplies the robot pose,
+   *                             provided by the odometry class.
+   * @param feedforward          The feedforward to use for the drivetrain.
+   * @param kinematics           The kinematics for the robot drivetrain.
+   * @param xController          The Trajectory Tracker PID controller
+   *                             for the robot's x position.
+   * @param yController          The Trajectory Tracker PID controller
+   *                             for the robot's y position.
+   * @param thetaController      The Trajectory Tracker PID controller
+   *                             for angle for the robot.
+   * @param desiredRotation      The angle that the robot should be facing.
+   *                             This is sampled at each time step.
+   * @param maxWheelVelocity     The maximum velocity of a drivetrain wheel.
+   * @param frontLeftController  The front left wheel velocity PID.
+   * @param rearLeftController   The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController  The rear right wheel velocity PID.
+   * @param currentWheelSpeeds   A MecanumDriveWheelSpeeds object containing
+   *                             the current wheel speeds.
+   * @param output               The output of the velocity PIDs.
+   * @param requirements         The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SimpleMotorFeedforward<units::meters> feedforward,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<frc::Rotation2d()> desiredRotation,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
+      frc2::PIDController frontLeftController,
+      frc2::PIDController rearLeftController,
+      frc2::PIDController frontRightController,
+      frc2::PIDController rearRightController,
+      std::function<void(units::volt_t, units::volt_t, units::volt_t,
+                         units::volt_t)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. PID control and feedforward are handled
+   * internally. Outputs are scaled from -12 to 12 as a voltage output to the
+   * motor.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory           The trajectory to follow.
    * @param pose                 A function that supplies the robot pose,
@@ -161,8 +271,48 @@
    * completion of the path - this is left to the user, since it is not
    * appropriate for paths with non-stationary end-states.
    *
-   * <p>Note2: The rotation controller will calculate the rotation based on the
-   * final pose in the trajectory, not the poses at each time step.
+   * @param trajectory       The trajectory to follow.
+   * @param pose             A function that supplies the robot pose - use one
+   * of the odometry classes to provide this.
+   * @param kinematics       The kinematics for the robot drivetrain.
+   * @param xController      The Trajectory Tracker PID controller
+   *                         for the robot's x position.
+   * @param yController      The Trajectory Tracker PID controller
+   *                         for the robot's y position.
+   * @param thetaController  The Trajectory Tracker PID controller
+   *                         for angle for the robot.
+   * @param desiredRotation  The angle that the robot should be facing.
+   *                         This is sampled at each time step.
+   * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+   * @param output           The output of the position PIDs.
+   * @param requirements     The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<frc::Rotation2d()> desiredRotation,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                         units::meters_per_second_t,
+                         units::meters_per_second_t)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. The user should implement a velocity PID on the
+   * desired output wheel velocities.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path - this is left to the user, since it is not
+   * appropriate for paths with non-stationary end-states.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory       The trajectory to follow.
    * @param pose             A function that supplies the robot pose - use one
@@ -199,8 +349,48 @@
    * completion of the path - this is left to the user, since it is not
    * appropriate for paths with non-stationary end-states.
    *
-   * <p>Note2: The rotation controller will calculate the rotation based on the
-   * final pose in the trajectory, not the poses at each time step.
+   * @param trajectory       The trajectory to follow.
+   * @param pose             A function that supplies the robot pose - use one
+   * of the odometry classes to provide this.
+   * @param kinematics       The kinematics for the robot drivetrain.
+   * @param xController      The Trajectory Tracker PID controller
+   *                         for the robot's x position.
+   * @param yController      The Trajectory Tracker PID controller
+   *                         for the robot's y position.
+   * @param thetaController  The Trajectory Tracker PID controller
+   *                         for angle for the robot.
+   * @param desiredRotation  The angle that the robot should be facing.
+   *                         This is sampled at every time step.
+   * @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
+   * @param output           The output of the position PIDs.
+   * @param requirements     The subsystems to require.
+   */
+  MecanumControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
+      frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<frc::Rotation2d()> desiredRotation,
+      units::meters_per_second_t maxWheelVelocity,
+      std::function<void(units::meters_per_second_t, units::meters_per_second_t,
+                         units::meters_per_second_t,
+                         units::meters_per_second_t)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * Constructs a new MecanumControllerCommand that when executed will follow
+   * the provided trajectory. The user should implement a velocity PID on the
+   * desired output wheel velocities.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path - this is left to the user, since it is not
+   * appropriate for paths with non-stationary end-states.
+   *
+   * <p>Note2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory       The trajectory to follow.
    * @param pose             A function that supplies the robot pose - use one
@@ -241,9 +431,8 @@
   std::function<frc::Pose2d()> m_pose;
   frc::SimpleMotorFeedforward<units::meters> m_feedforward;
   frc::MecanumDriveKinematics m_kinematics;
-  std::unique_ptr<frc2::PIDController> m_xController;
-  std::unique_ptr<frc2::PIDController> m_yController;
-  std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+  frc::HolonomicDriveController m_controller;
+  std::function<frc::Rotation2d()> m_desiredRotation;
   const units::meters_per_second_t m_maxWheelVelocity;
   std::unique_ptr<frc2::PIDController> m_frontLeftController;
   std::unique_ptr<frc2::PIDController> m_rearLeftController;
@@ -261,6 +450,5 @@
   frc2::Timer m_timer;
   frc::MecanumDriveWheelSpeeds m_prevSpeeds;
   units::second_t m_prevTime;
-  frc::Pose2d m_finalPose;
 };
 }  // namespace frc2

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
index 59af028..1c6d776 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
@@ -11,7 +11,7 @@
 #include <initializer_list>
 
 #include <frc/Notifier.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 
 #include "frc2/command/CommandBase.h"

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index 3a6df88..608fe56 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -38,6 +38,13 @@
   void SetSetpoint(double setpoint);
 
   /**
+   * Gets the setpoint for the subsystem.
+   *
+   * @return the setpoint for the subsystem
+   */
+  double GetSetpoint() const;
+
+  /**
    * Enables the PID control.  Resets the controller.
    */
   virtual void Enable();

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index b685230..5bc4978 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -12,7 +12,7 @@
 #include <utility>
 
 #include <frc/controller/ProfiledPIDController.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 
 #include "frc2/command/CommandBase.h"
@@ -96,11 +96,12 @@
                      std::function<Distance_t()> goalSource,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
-      : ProfiledPIDCommand(controller, measurementSource,
-                           [&goalSource]() {
-                             return State{goalSource(), Velocity_t{0}};
-                           },
-                           useOutput, requirements) {}
+      : ProfiledPIDCommand(
+            controller, measurementSource,
+            [&goalSource]() {
+              return State{goalSource(), Velocity_t{0}};
+            },
+            useOutput, requirements) {}
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -117,11 +118,12 @@
                      std::function<Distance_t()> goalSource,
                      std::function<void(double, State)> useOutput,
                      wpi::ArrayRef<Subsystem*> requirements = {})
-      : ProfiledPIDCommand(controller, measurementSource,
-                           [&goalSource]() {
-                             return State{goalSource(), Velocity_t{0}};
-                           },
-                           useOutput, requirements) {}
+      : ProfiledPIDCommand(
+            controller, measurementSource,
+            [&goalSource]() {
+              return State{goalSource(), Velocity_t{0}};
+            },
+            useOutput, requirements) {}
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -137,8 +139,9 @@
                      std::function<Distance_t()> measurementSource, State goal,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
-      : ProfiledPIDCommand(controller, measurementSource,
-                           [goal] { return goal; }, useOutput, requirements) {}
+      : ProfiledPIDCommand(
+            controller, measurementSource, [goal] { return goal; }, useOutput,
+            requirements) {}
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -154,8 +157,9 @@
                      std::function<Distance_t()> measurementSource, State goal,
                      std::function<void(double, State)> useOutput,
                      wpi::ArrayRef<Subsystem*> requirements = {})
-      : ProfiledPIDCommand(controller, measurementSource,
-                           [goal] { return goal; }, useOutput, requirements) {}
+      : ProfiledPIDCommand(
+            controller, measurementSource, [goal] { return goal; }, useOutput,
+            requirements) {}
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -172,8 +176,9 @@
                      Distance_t goal,
                      std::function<void(double, State)> useOutput,
                      std::initializer_list<Subsystem*> requirements)
-      : ProfiledPIDCommand(controller, measurementSource,
-                           [goal] { return goal; }, useOutput, requirements) {}
+      : ProfiledPIDCommand(
+            controller, measurementSource, [goal] { return goal; }, useOutput,
+            requirements) {}
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -190,8 +195,9 @@
                      Distance_t goal,
                      std::function<void(double, State)> useOutput,
                      wpi::ArrayRef<Subsystem*> requirements = {})
-      : ProfiledPIDCommand(controller, measurementSource,
-                           [goal] { return goal; }, useOutput, requirements) {}
+      : ProfiledPIDCommand(
+            controller, measurementSource, [goal] { return goal; }, useOutput,
+            requirements) {}
 
   ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
 

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
index 919ed18..2bbe149 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -8,7 +8,7 @@
 #pragma once
 
 #include <frc/controller/ProfiledPIDController.h>
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc2/command/SubsystemBase.h"
 

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
index 580bc57..21d5cfd 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
@@ -17,7 +17,8 @@
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/trajectory/Trajectory.h>
-#include <units/units.h>
+#include <units/length.h>
+#include <units/voltage.h>
 #include <wpi/ArrayRef.h>
 
 #include "frc2/Timer.h"

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
index c0c9096..9b1252e 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
@@ -17,7 +17,7 @@
 
 namespace frc2 {
 /**
- * A command that runs a given runnable when it is initalized, and another
+ * A command that runs a given runnable when it is initialized, and another
  * runnable when it ends. Useful for running and then stopping a motor, or
  * extending and then retracting a solenoid. Has no end condition as-is; either
  * subclass it or use Command.WithTimeout() or Command.WithInterrupt() to give

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
index 687510d..3f8a278 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -49,6 +49,14 @@
   virtual void Periodic();
 
   /**
+   * This method is called periodically by the CommandScheduler.  Useful for
+   * updating subsystem-specific state that needs to be maintained for
+   * simulations, such as for updating simulation classes and setting simulated
+   * sensor readings.
+   */
+  virtual void SimulationPeriodic();
+
+  /**
    * Sets the default Command of the subsystem.  The default command will be
    * automatically scheduled when no other commands are scheduled that require
    * the subsystem. Default commands should generally not end on their own, i.e.

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
index 38ca60c..6036ce8 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
@@ -10,6 +10,7 @@
 #include <initializer_list>
 #include <memory>
 
+#include <frc/controller/HolonomicDriveController.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Pose2d.h>
@@ -17,7 +18,9 @@
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/kinematics/SwerveModuleState.h>
 #include <frc/trajectory/Trajectory.h>
-#include <units/units.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/voltage.h>
 #include <wpi/ArrayRef.h>
 
 #include "CommandBase.h"
@@ -68,8 +71,46 @@
    * completion of the path- this is left to the user, since it is not
    * appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The rotation controller will calculate the rotation based on the
-   * final pose in the trajectory, not the poses at each time step.
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param xController     The Trajectory Tracker PID controller
+   *                        for the robot's x position.
+   * @param yController     The Trajectory Tracker PID controller
+   *                        for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller
+   *                        for angle for the robot.
+   * @param desiredRotation The angle that the drivetrain should be
+   *                        facing. This is sampled at each time step.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc2::PIDController xController, frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<frc::Rotation2d()> desiredRotation,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory      The trajectory to follow.
    * @param pose            A function that supplies the robot pose,
@@ -104,8 +145,47 @@
    * completion of the path- this is left to the user, since it is not
    * appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The rotation controller will calculate the rotation based on the
-   * final pose in the trajectory, not the poses at each time step.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param xController     The Trajectory Tracker PID controller
+   *                        for the robot's x position.
+   * @param yController     The Trajectory Tracker PID controller
+   *                        for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller
+   *                        for angle for the robot.
+   * @param desiredRotation The angle that the drivetrain should be
+   *                        facing. This is sampled at each time step.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc2::PIDController xController, frc2::PIDController yController,
+      frc::ProfiledPIDController<units::radians> thetaController,
+      std::function<frc::Rotation2d()> desiredRotation,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      wpi::ArrayRef<Subsystem*> requirements = {});
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
    *
    * @param trajectory      The trajectory to follow.
    * @param pose            A function that supplies the robot pose,
@@ -142,15 +222,15 @@
   frc::Trajectory m_trajectory;
   std::function<frc::Pose2d()> m_pose;
   frc::SwerveDriveKinematics<NumModules> m_kinematics;
-  std::unique_ptr<frc2::PIDController> m_xController;
-  std::unique_ptr<frc2::PIDController> m_yController;
-  std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
+  frc::HolonomicDriveController m_controller;
   std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
       m_outputStates;
 
+  std::function<frc::Rotation2d()> m_desiredRotation;
+
   frc2::Timer m_timer;
   units::second_t m_prevTime;
-  frc::Pose2d m_finalPose;
+  frc::Rotation2d m_finalRotation;
 };
 }  // namespace frc2
 

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
index 42d726d..616244e 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
@@ -17,16 +17,51 @@
     frc::SwerveDriveKinematics<NumModules> kinematics,
     frc2::PIDController xController, frc2::PIDController yController,
     frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<frc::Rotation2d()> desiredRotation,
     std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
     std::initializer_list<Subsystem*> requirements)
     : m_trajectory(trajectory),
       m_pose(pose),
       m_kinematics(kinematics),
-      m_xController(std::make_unique<frc2::PIDController>(xController)),
-      m_yController(std::make_unique<frc2::PIDController>(yController)),
-      m_thetaController(
-          std::make_unique<frc::ProfiledPIDController<units::radians>>(
-              thetaController)),
+      m_controller(xController, yController, thetaController),
+      m_desiredRotation(desiredRotation),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc2::PIDController xController, frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_controller(xController, yController, thetaController),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+  m_desiredRotation = [&] {
+    return m_trajectory.States().back().pose.Rotation();
+  };
+}
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc2::PIDController xController, frc2::PIDController yController,
+    frc::ProfiledPIDController<units::radians> thetaController,
+    std::function<frc::Rotation2d()> desiredRotation,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    wpi::ArrayRef<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_kinematics(kinematics),
+      m_controller(xController, yController, thetaController),
+      m_desiredRotation(desiredRotation),
       m_outputStates(output) {
   this->AddRequirements(requirements);
 }
@@ -42,19 +77,16 @@
     : m_trajectory(trajectory),
       m_pose(pose),
       m_kinematics(kinematics),
-      m_xController(std::make_unique<frc2::PIDController>(xController)),
-      m_yController(std::make_unique<frc2::PIDController>(yController)),
-      m_thetaController(
-          std::make_unique<frc::ProfiledPIDController<units::radians>>(
-              thetaController)),
+      m_controller(xController, yController, thetaController),
       m_outputStates(output) {
   this->AddRequirements(requirements);
+  m_desiredRotation = [&] {
+    return m_trajectory.States().back().pose.Rotation();
+  };
 }
 
 template <size_t NumModules>
 void SwerveControllerCommand<NumModules>::Initialize() {
-  m_finalPose = m_trajectory.Sample(m_trajectory.TotalTime()).pose;
-
   m_timer.Reset();
   m_timer.Start();
 }
@@ -62,34 +94,10 @@
 template <size_t NumModules>
 void SwerveControllerCommand<NumModules>::Execute() {
   auto curTime = units::second_t(m_timer.Get());
-
   auto m_desiredState = m_trajectory.Sample(curTime);
-  auto m_desiredPose = m_desiredState.pose;
-
-  auto m_poseError = m_desiredPose.RelativeTo(m_pose());
-
-  auto targetXVel = units::meters_per_second_t(m_xController->Calculate(
-      (m_pose().Translation().X().template to<double>()),
-      (m_desiredPose.Translation().X().template to<double>())));
-  auto targetYVel = units::meters_per_second_t(m_yController->Calculate(
-      (m_pose().Translation().Y().template to<double>()),
-      (m_desiredPose.Translation().Y().template to<double>())));
-
-  // Profiled PID Controller only takes meters as setpoint and measurement
-  // The robot will go to the desired rotation of the final pose in the
-  // trajectory, not following the poses at individual states.
-  auto targetAngularVel =
-      units::radians_per_second_t(m_thetaController->Calculate(
-          m_pose().Rotation().Radians(), m_finalPose.Rotation().Radians()));
-
-  auto vRef = m_desiredState.velocity;
-
-  targetXVel += vRef * m_poseError.Rotation().Cos();
-  targetYVel += vRef * m_poseError.Rotation().Sin();
 
   auto targetChassisSpeeds =
-      frc::ChassisSpeeds{targetXVel, targetYVel, targetAngularVel};
-
+      m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
   auto targetModuleStates =
       m_kinematics.ToSwerveModuleStates(targetChassisSpeeds);
 
@@ -103,7 +111,7 @@
 
 template <size_t NumModules>
 bool SwerveControllerCommand<NumModules>::IsFinished() {
-  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+  return m_timer.HasElapsed(m_trajectory.TotalTime());
 }
 
 }  // namespace frc2

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index a83dc39..70bf7f9 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -72,7 +72,7 @@
   void End(bool interrupted) override { m_timer.Stop(); }
 
   bool IsFinished() override {
-    return m_timer.HasPeriodPassed(m_profile.TotalTime());
+    return m_timer.HasElapsed(m_profile.TotalTime());
   }
 
  private:

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
index c5d7e35..08336f1 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,7 @@
 #pragma once
 
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc2/command/SubsystemBase.h"
 

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
index ab97958..568db65 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc2/Timer.h"
 #include "frc2/command/CommandBase.h"

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
index 50b4855..741d3eb 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <functional>
 
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
index af861d4..5e01edf 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
@@ -86,7 +86,7 @@
 
   /**
    * Binds a command to be started repeatedly while the button is pressed, and
-   * cancelled when it is released.  Takes a raw pointer, and so is non-owning;
+   * canceled when it is released.  Takes a raw pointer, and so is non-owning;
    * users are responsible for the lifespan of the command.
    *
    * @param command The command to bind.
@@ -97,7 +97,7 @@
 
   /**
    * Binds a command to be started repeatedly while the button is pressed, and
-   * cancelled when it is released.  Transfers command ownership to the button
+   * canceled when it is released.  Transfers command ownership to the button
    * scheduler, so the user does not have to worry about lifespan - rvalue refs
    * will be *moved*, lvalue refs will be *copied.*
    *
@@ -131,7 +131,7 @@
                    wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
-   * Binds a command to be started when the button is pressed, and cancelled
+   * Binds a command to be started when the button is pressed, and canceled
    * when it is released.  Takes a raw pointer, and so is non-owning; users are
    * responsible for the lifespan of the command.
    *
@@ -142,7 +142,7 @@
   Button WhenHeld(Command* command, bool interruptible = true);
 
   /**
-   * Binds a command to be started when the button is pressed, and cancelled
+   * Binds a command to be started when the button is pressed, and canceled
    * when it is released.  Transfers command ownership to the button scheduler,
    * so the user does not have to worry about lifespan - rvalue refs will be
    * *moved*, lvalue refs will be *copied.*
@@ -205,7 +205,7 @@
                       wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
-   * Binds a command to start when the button is pressed, and be cancelled when
+   * Binds a command to start when the button is pressed, and be canceled when
    * it is pressed again.  Takes a raw pointer, and so is non-owning; users are
    * responsible for the lifespan of the command.
    *
@@ -216,7 +216,7 @@
   Button ToggleWhenPressed(Command* command, bool interruptible = true);
 
   /**
-   * Binds a command to start when the button is pressed, and be cancelled when
+   * Binds a command to start when the button is pressed, and be canceled when
    * it is pessed again.  Transfers command ownership to the button scheduler,
    * so the user does not have to worry about lifespan - rvalue refs will be
    * *moved*, lvalue refs will be *copied.*
@@ -233,7 +233,7 @@
   }
 
   /**
-   * Binds a command to be cancelled when the button is pressed.  Takes a
+   * Binds a command to be canceled when the button is pressed.  Takes a
    * raw pointer, and so is non-owning; users are responsible for the lifespan
    *  and scheduling of the command.
    *

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
index a23738b..421562e 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -26,12 +26,8 @@
    * @param buttonNumber The number of the button on the joystic.
    */
   explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
-      : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
-
-  bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
-
- private:
-  frc::GenericHID* m_joystick;
-  int m_buttonNumber;
+      : Button([joystick, buttonNumber] {
+          return joystick->GetRawButton(buttonNumber);
+        }) {}
 };
 }  // namespace frc2

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/NetworkButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/NetworkButton.h
new file mode 100644
index 0000000..9c09dd1
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/NetworkButton.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/Twine.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A {@link Button} that uses a {@link NetworkTable} boolean field.
+ */
+class NetworkButton : public Button {
+ public:
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param entry The entry that is the value.
+   */
+  explicit NetworkButton(nt::NetworkTableEntry entry);
+
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param table The table where the networktable value is located.
+   * @param field The field that is the value.
+   */
+  NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+                const wpi::Twine& field);
+
+  /**
+   * Creates a NetworkButton that commands can be bound to.
+   *
+   * @param table The table where the networktable value is located.
+   * @param field The field that is the value.
+   */
+  NetworkButton(const wpi::Twine& table, const wpi::Twine& field);
+};
+}  // namespace frc2

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
index 758cab2..e4f8f0e 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -27,15 +27,8 @@
    * @param povNumber The number of the POV on the joystick.
    */
   POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
-      : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
-
-  bool Get() const override {
-    return m_joystick->GetPOV(m_povNumber) == m_angle;
-  }
-
- private:
-  frc::GenericHID* m_joystick;
-  int m_angle;
-  int m_povNumber;
+      : Button([joystick, angle, povNumber] {
+          return joystick->GetPOV(povNumber) == angle;
+        }) {}
 };
 }  // namespace frc2

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

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
index 7df6b4e..ecfc4d1 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -48,13 +48,6 @@
   Trigger(const Trigger& other);
 
   /**
-   * Returns whether the trigger is active.  Can be overridden by a subclass.
-   *
-   * @return Whether the trigger is active.
-   */
-  virtual bool Get() const { return m_isActive(); }
-
-  /**
    * Binds a command to start when the trigger becomes active.  Takes a
    * raw pointer, and so is non-owning; users are responsible for the lifespan
    * of the command.
@@ -79,11 +72,11 @@
                          Command, std::remove_reference_t<T>>>>
   Trigger WhenActive(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (!pressedLast && pressed) {
             command->Schedule(interruptible);
@@ -115,7 +108,7 @@
 
   /**
    * Binds a command to be started repeatedly while the trigger is active, and
-   * cancelled when it becomes inactive.  Takes a raw pointer, and so is
+   * canceled when it becomes inactive.  Takes a raw pointer, and so is
    * non-owning; users are responsible for the lifespan of the command.
    *
    * @param command The command to bind.
@@ -126,7 +119,7 @@
 
   /**
    * Binds a command to be started repeatedly while the trigger is active, and
-   * cancelled when it becomes inactive.  Transfers command ownership to the
+   * canceled when it becomes inactive.  Transfers command ownership to the
    * button scheduler, so the user does not have to worry about lifespan -
    * rvalue refs will be *moved*, lvalue refs will be *copied.*
    *
@@ -138,11 +131,11 @@
                          Command, std::remove_reference_t<T>>>>
   Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (pressed) {
             command->Schedule(interruptible);
@@ -175,7 +168,7 @@
 
   /**
    * Binds a command to be started when the trigger becomes active, and
-   * cancelled when it becomes inactive.  Takes a raw pointer, and so is
+   * canceled when it becomes inactive.  Takes a raw pointer, and so is
    * non-owning; users are responsible for the lifespan of the command.
    *
    * @param command The command to bind.
@@ -186,7 +179,7 @@
 
   /**
    * Binds a command to be started when the trigger becomes active, and
-   * cancelled when it becomes inactive.  Transfers command ownership to the
+   * canceled when it becomes inactive.  Transfers command ownership to the
    * button scheduler, so the user does not have to worry about lifespan -
    * rvalue refs will be *moved*, lvalue refs will be *copied.*
    *
@@ -198,11 +191,11 @@
                          Command, std::remove_reference_t<T>>>>
   Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (!pressedLast && pressed) {
             command->Schedule(interruptible);
@@ -240,11 +233,11 @@
                          Command, std::remove_reference_t<T>>>>
   Trigger WhenInactive(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (pressedLast && !pressed) {
             command->Schedule(interruptible);
@@ -274,7 +267,7 @@
                        wpi::ArrayRef<Subsystem*> requirements = {});
 
   /**
-   * Binds a command to start when the trigger becomes active, and be cancelled
+   * Binds a command to start when the trigger becomes active, and be canceled
    * when it again becomes active.  Takes a raw pointer, and so is non-owning;
    * users are responsible for the lifespan of the command.
    *
@@ -285,7 +278,7 @@
   Trigger ToggleWhenActive(Command* command, bool interruptible = true);
 
   /**
-   * Binds a command to start when the trigger becomes active, and be cancelled
+   * Binds a command to start when the trigger becomes active, and be canceled
    * when it again becomes active.  Transfers command ownership to the button
    * scheduler, so the user does not have to worry about lifespan - rvalue refs
    * will be *moved*, lvalue refs will be *copied.*
@@ -298,11 +291,11 @@
                          Command, std::remove_reference_t<T>>>>
   Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
     CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
+        [pressedLast = m_isActive(), *this,
          command = std::make_unique<std::remove_reference_t<T>>(
              std::forward<T>(command)),
          interruptible]() mutable {
-          bool pressed = Get();
+          bool pressed = m_isActive();
 
           if (!pressedLast && pressed) {
             if (command->IsScheduled()) {
@@ -318,7 +311,7 @@
   }
 
   /**
-   * Binds a command to be cancelled when the trigger becomes active.  Takes a
+   * Binds a command to be canceled when the trigger becomes active.  Takes a
    * raw pointer, and so is non-owning; users are responsible for the lifespan
    *  and scheduling of the command.
    *
@@ -333,7 +326,7 @@
    * @return A trigger which is active when both component triggers are active.
    */
   Trigger operator&&(Trigger rhs) {
-    return Trigger([*this, rhs] { return Get() && rhs.Get(); });
+    return Trigger([*this, rhs] { return m_isActive() && rhs.m_isActive(); });
   }
 
   /**
@@ -342,7 +335,7 @@
    * @return A trigger which is active when either component trigger is active.
    */
   Trigger operator||(Trigger rhs) {
-    return Trigger([*this, rhs] { return Get() || rhs.Get(); });
+    return Trigger([*this, rhs] { return m_isActive() || rhs.m_isActive(); });
   }
 
   /**
@@ -352,7 +345,7 @@
    * and vice-versa.
    */
   Trigger operator!() {
-    return Trigger([*this] { return !Get(); });
+    return Trigger([*this] { return !m_isActive(); });
   }
 
  private:

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
index 4f9abf0..0547b66 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
 
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 
 public final class MockHardwareExtension implements BeforeAllCallback {
   private static ExtensionContext getRoot(ExtensionContext context) {
@@ -21,7 +21,7 @@
 
   @Override
   public void beforeAll(ExtensionContext context) {
-    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initalized", key -> {
+    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initialized", key -> {
       initializeHardware();
       return true;
     }, Boolean.class);
@@ -29,12 +29,9 @@
 
   private void initializeHardware() {
     HAL.initialize(500, 0);
-    DriverStationSim dsSim = new DriverStationSim();
-    dsSim.setDsAttached(true);
-    dsSim.setAutonomous(false);
-    dsSim.setEnabled(true);
-    dsSim.setTest(true);
-
-
+    DriverStationSim.setDsAttached(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setTest(true);
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
deleted file mode 100644
index 64354c3..0000000
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java
+++ /dev/null
@@ -1,179 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj2.command;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj2.command.button.InternalButton;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-import static org.mockito.Mockito.never;
-import static org.mockito.Mockito.times;
-import static org.mockito.Mockito.verify;
-import static org.mockito.Mockito.when;
-
-
-class ButtonTest extends CommandTestBase {
-  @Test
-  void whenPressedTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-
-    InternalButton button = new InternalButton();
-    button.setPressed(false);
-    button.whenPressed(command1);
-    scheduler.run();
-    verify(command1, never()).schedule(true);
-    button.setPressed(true);
-    scheduler.run();
-    scheduler.run();
-    verify(command1).schedule(true);
-  }
-
-  @Test
-  void whenReleasedTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-
-    InternalButton button = new InternalButton();
-    button.setPressed(true);
-    button.whenReleased(command1);
-    scheduler.run();
-    verify(command1, never()).schedule(true);
-    button.setPressed(false);
-    scheduler.run();
-    scheduler.run();
-    verify(command1).schedule(true);
-  }
-
-  @Test
-  void whileHeldTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-
-    InternalButton button = new InternalButton();
-    button.setPressed(false);
-    button.whileHeld(command1);
-    scheduler.run();
-    verify(command1, never()).schedule(true);
-    button.setPressed(true);
-    scheduler.run();
-    scheduler.run();
-    verify(command1, times(2)).schedule(true);
-    button.setPressed(false);
-    scheduler.run();
-    verify(command1).cancel();
-  }
-
-  @Test
-  void whenHeldTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-
-    InternalButton button = new InternalButton();
-    button.setPressed(false);
-    button.whenHeld(command1);
-    scheduler.run();
-    verify(command1, never()).schedule(true);
-    button.setPressed(true);
-    scheduler.run();
-    scheduler.run();
-    verify(command1).schedule(true);
-    button.setPressed(false);
-    scheduler.run();
-    verify(command1).cancel();
-  }
-
-  @Test
-  void toggleWhenPressedTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-
-    InternalButton button = new InternalButton();
-    button.setPressed(false);
-    button.toggleWhenPressed(command1);
-    scheduler.run();
-    verify(command1, never()).schedule(true);
-    button.setPressed(true);
-    scheduler.run();
-    when(command1.isScheduled()).thenReturn(true);
-    scheduler.run();
-    verify(command1).schedule(true);
-    button.setPressed(false);
-    scheduler.run();
-    verify(command1, never()).cancel();
-    button.setPressed(true);
-    scheduler.run();
-    verify(command1).cancel();
-  }
-
-  @Test
-  void cancelWhenPressedTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-
-    InternalButton button = new InternalButton();
-    button.setPressed(false);
-    button.cancelWhenPressed(command1);
-    scheduler.run();
-    verify(command1, never()).cancel();
-    button.setPressed(true);
-    scheduler.run();
-    scheduler.run();
-    verify(command1).cancel();
-  }
-
-  @Test
-  void runnableBindingTest() {
-
-    InternalButton buttonWhenPressed = new InternalButton();
-    InternalButton buttonWhileHeld = new InternalButton();
-    InternalButton buttonWhenReleased = new InternalButton();
-
-    buttonWhenPressed.setPressed(false);
-    buttonWhileHeld.setPressed(true);
-    buttonWhenReleased.setPressed(true);
-
-    Counter counter = new Counter();
-
-    buttonWhenPressed.whenPressed(counter::increment);
-    buttonWhileHeld.whileHeld(counter::increment);
-    buttonWhenReleased.whenReleased(counter::increment);
-
-    CommandScheduler scheduler = CommandScheduler.getInstance();
-
-    scheduler.run();
-    buttonWhenPressed.setPressed(true);
-    buttonWhenReleased.setPressed(false);
-    scheduler.run();
-
-    assertEquals(counter.m_counter, 4);
-  }
-
-  @Test
-  void buttonCompositionTest() {
-    InternalButton button1 = new InternalButton();
-    InternalButton button2 = new InternalButton();
-
-    button1.setPressed(true);
-    button2.setPressed(false);
-
-    assertFalse(button1.and(button2).get());
-    assertTrue(button1.or(button2).get());
-    assertFalse(button1.negate().get());
-    assertTrue(button1.and(button2.negate()).get());
-  }
-}

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
index 23f63c7..d661598 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,175 +8,182 @@
 package edu.wpi.first.wpilibj2.command;
 
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 class CommandDecoratorTest extends CommandTestBase {
   @Test
+  @ResourceLock("timing")
   void withTimeoutTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command command1 = new WaitCommand(10);
 
-    Command command1 = new WaitCommand(10);
+      Command timeout = command1.withTimeout(2);
 
-    Command timeout = command1.withTimeout(2);
+      scheduler.schedule(timeout);
+      scheduler.run();
 
-    scheduler.schedule(timeout);
-    scheduler.run();
+      assertFalse(scheduler.isScheduled(command1));
+      assertTrue(scheduler.isScheduled(timeout));
 
-    assertFalse(scheduler.isScheduled(command1));
-    assertTrue(scheduler.isScheduled(timeout));
+      SimHooks.stepTiming(3);
+      scheduler.run();
 
-    Timer.delay(3);
-    scheduler.run();
-
-    assertFalse(scheduler.isScheduled(timeout));
+      assertFalse(scheduler.isScheduled(timeout));
+    } finally {
+      SimHooks.resumeTiming();
+    }
   }
 
   @Test
   void withInterruptTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder condition = new ConditionHolder();
 
-    ConditionHolder condition = new ConditionHolder();
+      Command command = new WaitCommand(10).withInterrupt(condition::getCondition);
 
-    Command command = new WaitCommand(10).withInterrupt(condition::getCondition);
-
-    scheduler.schedule(command);
-    scheduler.run();
-    assertTrue(scheduler.isScheduled(command));
-    condition.setCondition(true);
-    scheduler.run();
-    assertFalse(scheduler.isScheduled(command));
+      scheduler.schedule(command);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command));
+      condition.setCondition(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(command));
+    }
   }
 
   @Test
   void beforeStartingTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder condition = new ConditionHolder();
+      condition.setCondition(false);
 
-    ConditionHolder condition = new ConditionHolder();
-    condition.setCondition(false);
+      Command command = new InstantCommand();
 
-    Command command = new InstantCommand();
+      scheduler.schedule(command.beforeStarting(() -> condition.setCondition(true)));
 
-    scheduler.schedule(command.beforeStarting(() -> condition.setCondition(true)));
-
-    assertTrue(condition.getCondition());
+      assertTrue(condition.getCondition());
+    }
   }
 
   @Test
   void andThenLambdaTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder condition = new ConditionHolder();
+      condition.setCondition(false);
 
-    ConditionHolder condition = new ConditionHolder();
-    condition.setCondition(false);
+      Command command = new InstantCommand();
 
-    Command command = new InstantCommand();
+      scheduler.schedule(command.andThen(() -> condition.setCondition(true)));
 
-    scheduler.schedule(command.andThen(() -> condition.setCondition(true)));
+      assertFalse(condition.getCondition());
 
-    assertFalse(condition.getCondition());
+      scheduler.run();
 
-    scheduler.run();
-
-    assertTrue(condition.getCondition());
+      assertTrue(condition.getCondition());
+    }
   }
 
   @Test
   void andThenTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder condition = new ConditionHolder();
+      condition.setCondition(false);
 
-    ConditionHolder condition = new ConditionHolder();
-    condition.setCondition(false);
+      Command command1 = new InstantCommand();
+      Command command2 = new InstantCommand(() -> condition.setCondition(true));
 
-    Command command1 = new InstantCommand();
-    Command command2 = new InstantCommand(() -> condition.setCondition(true));
+      scheduler.schedule(command1.andThen(command2));
 
-    scheduler.schedule(command1.andThen(command2));
+      assertFalse(condition.getCondition());
 
-    assertFalse(condition.getCondition());
+      scheduler.run();
 
-    scheduler.run();
-
-    assertTrue(condition.getCondition());
+      assertTrue(condition.getCondition());
+    }
   }
 
   @Test
   void deadlineWithTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder condition = new ConditionHolder();
+      condition.setCondition(false);
 
-    ConditionHolder condition = new ConditionHolder();
-    condition.setCondition(false);
+      Command dictator = new WaitUntilCommand(condition::getCondition);
+      Command endsBefore = new InstantCommand();
+      Command endsAfter = new WaitUntilCommand(() -> false);
 
-    Command dictator = new WaitUntilCommand(condition::getCondition);
-    Command endsBefore = new InstantCommand();
-    Command endsAfter = new WaitUntilCommand(() -> false);
+      Command group = dictator.deadlineWith(endsBefore, endsAfter);
 
-    Command group = dictator.deadlineWith(endsBefore, endsAfter);
+      scheduler.schedule(group);
+      scheduler.run();
 
-    scheduler.schedule(group);
-    scheduler.run();
+      assertTrue(scheduler.isScheduled(group));
 
-    assertTrue(scheduler.isScheduled(group));
+      condition.setCondition(true);
+      scheduler.run();
 
-    condition.setCondition(true);
-    scheduler.run();
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void alongWithTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder condition = new ConditionHolder();
+      condition.setCondition(false);
 
-    ConditionHolder condition = new ConditionHolder();
-    condition.setCondition(false);
+      Command command1 = new WaitUntilCommand(condition::getCondition);
+      Command command2 = new InstantCommand();
 
-    Command command1 = new WaitUntilCommand(condition::getCondition);
-    Command command2 = new InstantCommand();
+      Command group = command1.alongWith(command2);
 
-    Command group = command1.alongWith(command2);
+      scheduler.schedule(group);
+      scheduler.run();
 
-    scheduler.schedule(group);
-    scheduler.run();
+      assertTrue(scheduler.isScheduled(group));
 
-    assertTrue(scheduler.isScheduled(group));
+      condition.setCondition(true);
+      scheduler.run();
 
-    condition.setCondition(true);
-    scheduler.run();
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void raceWithTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command command1 = new WaitUntilCommand(() -> false);
+      Command command2 = new InstantCommand();
 
-    Command command1 = new WaitUntilCommand(() -> false);
-    Command command2 = new InstantCommand();
+      Command group = command1.raceWith(command2);
 
-    Command group = command1.raceWith(command2);
+      scheduler.schedule(group);
+      scheduler.run();
 
-    scheduler.schedule(group);
-    scheduler.run();
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void perpetuallyTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command command = new InstantCommand();
 
-    Command command = new InstantCommand();
+      Command perpetual = command.perpetually();
 
-    Command perpetual = command.perpetually();
+      scheduler.schedule(perpetual);
+      scheduler.run();
+      scheduler.run();
+      scheduler.run();
 
-    scheduler.schedule(perpetual);
-    scheduler.run();
-    scheduler.run();
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(perpetual));
+      assertTrue(scheduler.isScheduled(perpetual));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
index e453d94..82e78c5 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -29,18 +29,18 @@
 
   @Test
   void commandInGroupExternallyScheduledTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      @SuppressWarnings("PMD.UnusedLocalVariable")
+      Command group = new ParallelCommandGroup(command1, command2);
 
-    @SuppressWarnings("PMD.UnusedLocalVariable")
-    Command group = new ParallelCommandGroup(command1, command2);
-
-    assertThrows(IllegalArgumentException.class,
-        () -> scheduler.schedule(command1));
+      assertThrows(IllegalArgumentException.class,
+          () -> scheduler.schedule(command1));
+    }
   }
 
   @Test

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
index 10d3ee0..d19331b 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,66 +14,65 @@
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.verify;
 
-@SuppressWarnings("PMD.TooManyMethods")
 class CommandRequirementsTest extends CommandTestBase {
   @Test
   void requirementInterruptTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem requirement = new TestSubsystem();
 
-    Subsystem requirement = new TestSubsystem();
+      MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+      Command interrupted = interruptedHolder.getMock();
+      MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+      Command interrupter = interrupterHolder.getMock();
 
-    MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
-    Command interrupted = interruptedHolder.getMock();
-    MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
-    Command interrupter = interrupterHolder.getMock();
+      scheduler.schedule(interrupted);
+      scheduler.run();
+      scheduler.schedule(interrupter);
+      scheduler.run();
 
-    scheduler.schedule(interrupted);
-    scheduler.run();
-    scheduler.schedule(interrupter);
-    scheduler.run();
+      verify(interrupted).initialize();
+      verify(interrupted).execute();
+      verify(interrupted).end(true);
 
-    verify(interrupted).initialize();
-    verify(interrupted).execute();
-    verify(interrupted).end(true);
+      verify(interrupter).initialize();
+      verify(interrupter).execute();
 
-    verify(interrupter).initialize();
-    verify(interrupter).execute();
-
-    assertFalse(scheduler.isScheduled(interrupted));
-    assertTrue(scheduler.isScheduled(interrupter));
+      assertFalse(scheduler.isScheduled(interrupted));
+      assertTrue(scheduler.isScheduled(interrupter));
+    }
   }
 
   @Test
   void requirementUninterruptibleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem requirement = new TestSubsystem();
 
-    Subsystem requirement = new TestSubsystem();
+      MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+      Command notInterrupted = interruptedHolder.getMock();
+      MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+      Command interrupter = interrupterHolder.getMock();
 
-    MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
-    Command notInterrupted = interruptedHolder.getMock();
-    MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
-    Command interrupter = interrupterHolder.getMock();
+      scheduler.schedule(false, notInterrupted);
+      scheduler.schedule(interrupter);
 
-    scheduler.schedule(false, notInterrupted);
-    scheduler.schedule(interrupter);
-
-    assertTrue(scheduler.isScheduled(notInterrupted));
-    assertFalse(scheduler.isScheduled(interrupter));
+      assertTrue(scheduler.isScheduled(notInterrupted));
+      assertFalse(scheduler.isScheduled(interrupter));
+    }
   }
 
   @Test
   void defaultCommandRequirementErrorTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem system = new TestSubsystem();
 
-    Subsystem system = new TestSubsystem();
+      Command missingRequirement = new WaitUntilCommand(() -> false);
+      Command ends = new InstantCommand(() -> {
+      }, system);
 
-    Command missingRequirement = new WaitUntilCommand(() -> false);
-    Command ends = new InstantCommand(() -> {
-    }, system);
-
-    assertThrows(IllegalArgumentException.class,
-        () -> scheduler.setDefaultCommand(system, missingRequirement));
-    assertThrows(IllegalArgumentException.class,
-        () -> scheduler.setDefaultCommand(system, ends));
+      assertThrows(IllegalArgumentException.class,
+          () -> scheduler.setDefaultCommand(system, missingRequirement));
+      assertThrows(IllegalArgumentException.class,
+          () -> scheduler.setDefaultCommand(system, ends));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
index aee30db..74d2d5d 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,104 +19,104 @@
 class CommandScheduleTest extends CommandTestBase {
   @Test
   void instantScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      holder.setFinished(true);
+      Command mockCommand = holder.getMock();
 
-    MockCommandHolder holder = new MockCommandHolder(true);
-    holder.setFinished(true);
-    Command mockCommand = holder.getMock();
+      scheduler.schedule(mockCommand);
+      assertTrue(scheduler.isScheduled(mockCommand));
+      verify(mockCommand).initialize();
 
-    scheduler.schedule(mockCommand);
-    assertTrue(scheduler.isScheduled(mockCommand));
-    verify(mockCommand).initialize();
+      scheduler.run();
 
-    scheduler.run();
+      verify(mockCommand).execute();
+      verify(mockCommand).end(false);
 
-    verify(mockCommand).execute();
-    verify(mockCommand).end(false);
-
-    assertFalse(scheduler.isScheduled(mockCommand));
+      assertFalse(scheduler.isScheduled(mockCommand));
+    }
   }
 
   @Test
   void singleIterationScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
 
-    MockCommandHolder holder = new MockCommandHolder(true);
-    Command mockCommand = holder.getMock();
+      scheduler.schedule(mockCommand);
 
-    scheduler.schedule(mockCommand);
+      assertTrue(scheduler.isScheduled(mockCommand));
 
-    assertTrue(scheduler.isScheduled(mockCommand));
+      scheduler.run();
+      holder.setFinished(true);
+      scheduler.run();
 
-    scheduler.run();
-    holder.setFinished(true);
-    scheduler.run();
+      verify(mockCommand).initialize();
+      verify(mockCommand, times(2)).execute();
+      verify(mockCommand).end(false);
 
-    verify(mockCommand).initialize();
-    verify(mockCommand, times(2)).execute();
-    verify(mockCommand).end(false);
-
-    assertFalse(scheduler.isScheduled(mockCommand));
+      assertFalse(scheduler.isScheduled(mockCommand));
+    }
   }
 
   @Test
   void multiScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true);
-    Command command3 = command3Holder.getMock();
+      scheduler.schedule(true, command1, command2, command3);
+      assertTrue(scheduler.isScheduled(command1, command2, command3));
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command1, command2, command3));
 
-    scheduler.schedule(true, command1, command2, command3);
-    assertTrue(scheduler.isScheduled(command1, command2, command3));
-    scheduler.run();
-    assertTrue(scheduler.isScheduled(command1, command2, command3));
+      command1Holder.setFinished(true);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command2, command3));
+      assertFalse(scheduler.isScheduled(command1));
 
-    command1Holder.setFinished(true);
-    scheduler.run();
-    assertTrue(scheduler.isScheduled(command2, command3));
-    assertFalse(scheduler.isScheduled(command1));
+      command2Holder.setFinished(true);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command3));
+      assertFalse(scheduler.isScheduled(command1, command2));
 
-    command2Holder.setFinished(true);
-    scheduler.run();
-    assertTrue(scheduler.isScheduled(command3));
-    assertFalse(scheduler.isScheduled(command1, command2));
-
-    command3Holder.setFinished(true);
-    scheduler.run();
-    assertFalse(scheduler.isScheduled(command1, command2, command3));
+      command3Holder.setFinished(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(command1, command2, command3));
+    }
   }
 
   @Test
   void schedulerCancelTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
 
-    MockCommandHolder holder = new MockCommandHolder(true);
-    Command mockCommand = holder.getMock();
+      scheduler.schedule(mockCommand);
 
-    scheduler.schedule(mockCommand);
+      scheduler.run();
+      scheduler.cancel(mockCommand);
+      scheduler.run();
 
-    scheduler.run();
-    scheduler.cancel(mockCommand);
-    scheduler.run();
+      verify(mockCommand).execute();
+      verify(mockCommand).end(true);
+      verify(mockCommand, never()).end(false);
 
-    verify(mockCommand).execute();
-    verify(mockCommand).end(true);
-    verify(mockCommand, never()).end(false);
-
-    assertFalse(scheduler.isScheduled(mockCommand));
+      assertFalse(scheduler.isScheduled(mockCommand));
+    }
   }
 
   @Test
   void notScheduledCancelTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
 
-    MockCommandHolder holder = new MockCommandHolder(true);
-    Command mockCommand = holder.getMock();
-
-    assertDoesNotThrow(() -> scheduler.cancel(mockCommand));
+      assertDoesNotThrow(() -> scheduler.cancel(mockCommand));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
index 61aa1a0..6500f69 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,8 +11,8 @@
 
 import org.junit.jupiter.api.BeforeEach;
 
-import edu.wpi.first.hal.sim.DriverStationSim;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 
 import static org.mockito.Mockito.mock;
 import static org.mockito.Mockito.when;
@@ -21,7 +21,7 @@
  * Basic setup for all {@link Command tests}."
  */
 @SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
-abstract class CommandTestBase {
+public abstract class CommandTestBase {
   @BeforeEach
   void commandSetup() {
     CommandScheduler.getInstance().cancelAll();
@@ -32,12 +32,11 @@
     setDSEnabled(true);
   }
 
-  void setDSEnabled(boolean enabled) {
-    DriverStationSim sim = new DriverStationSim();
-    sim.setDsAttached(true);
+  public void setDSEnabled(boolean enabled) {
+    DriverStationSim.setDsAttached(true);
 
-    sim.setEnabled(enabled);
-    sim.notifyNewData();
+    DriverStationSim.setEnabled(enabled);
+    DriverStationSim.notifyNewData();
     DriverStation.getInstance().isNewControlData();
     while (DriverStation.getInstance().isEnabled() != enabled) {
       try {
@@ -48,44 +47,44 @@
     }
   }
 
-  class TestSubsystem extends SubsystemBase {
+  public class TestSubsystem extends SubsystemBase {
   }
 
-  protected class MockCommandHolder {
+  public class MockCommandHolder {
     private final Command m_mockCommand = mock(Command.class);
 
-    MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) {
+    public MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) {
       when(m_mockCommand.getRequirements()).thenReturn(Set.of(requirements));
       when(m_mockCommand.isFinished()).thenReturn(false);
       when(m_mockCommand.runsWhenDisabled()).thenReturn(runWhenDisabled);
     }
 
-    Command getMock() {
+    public Command getMock() {
       return m_mockCommand;
     }
 
-    void setFinished(boolean finished) {
+    public void setFinished(boolean finished) {
       when(m_mockCommand.isFinished()).thenReturn(finished);
     }
 
   }
 
-  protected class Counter {
-    int m_counter;
+  public class Counter {
+    public int m_counter;
 
-    void increment() {
+    public void increment() {
       m_counter++;
     }
   }
 
-  protected class ConditionHolder {
+  public class ConditionHolder {
     private boolean m_condition;
 
-    void setCondition(boolean condition) {
+    public void setCondition(boolean condition) {
       m_condition = condition;
     }
 
-    boolean getCondition() {
+    public boolean getCondition() {
       return m_condition;
     }
   }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
index 0740565..939fa01 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,26 +16,27 @@
 class ConditionalCommandTest extends CommandTestBase {
   @Test
   void conditionalCommandTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      command1Holder.setFinished(true);
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    command1Holder.setFinished(true);
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      ConditionalCommand conditionalCommand =
+          new ConditionalCommand(command1, command2, () -> true);
 
-    ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+      scheduler.schedule(conditionalCommand);
+      scheduler.run();
 
-    scheduler.schedule(conditionalCommand);
-    scheduler.run();
+      verify(command1).initialize();
+      verify(command1).execute();
+      verify(command1).end(false);
 
-    verify(command1).initialize();
-    verify(command1).execute();
-    verify(command1).end(false);
-
-    verify(command2, never()).initialize();
-    verify(command2, never()).execute();
-    verify(command2, never()).end(false);
+      verify(command2, never()).initialize();
+      verify(command2, never()).execute();
+      verify(command2, never()).end(false);
+    }
   }
 
   @Test
@@ -44,22 +45,23 @@
     Subsystem system2 = new TestSubsystem();
     Subsystem system3 = new TestSubsystem();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
-    Command command2 = command2Holder.getMock();
+      ConditionalCommand conditionalCommand =
+          new ConditionalCommand(command1, command2, () -> true);
 
-    ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true);
+      scheduler.schedule(conditionalCommand);
+      scheduler.schedule(new InstantCommand(() -> {
+      }, system3));
 
-    scheduler.schedule(conditionalCommand);
-    scheduler.schedule(new InstantCommand(() -> {
-    }, system3));
+      assertFalse(scheduler.isScheduled(conditionalCommand));
 
-    assertFalse(scheduler.isScheduled(conditionalCommand));
-
-    verify(command1).end(true);
-    verify(command2, never()).end(true);
+      verify(command1).end(true);
+      verify(command2, never()).end(true);
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
index 033dcc5..dccf0ff 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,68 +16,68 @@
 class DefaultCommandTest extends CommandTestBase {
   @Test
   void defaultCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      TestSubsystem hasDefaultCommand = new TestSubsystem();
 
-    TestSubsystem hasDefaultCommand = new TestSubsystem();
+      MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+      Command defaultCommand = defaultHolder.getMock();
 
-    MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
-    Command defaultCommand = defaultHolder.getMock();
+      scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+      scheduler.run();
 
-    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(defaultCommand));
+      assertTrue(scheduler.isScheduled(defaultCommand));
+    }
   }
 
   @Test
   void defaultCommandInterruptResumeTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      TestSubsystem hasDefaultCommand = new TestSubsystem();
 
-    TestSubsystem hasDefaultCommand = new TestSubsystem();
+      MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+      Command defaultCommand = defaultHolder.getMock();
+      MockCommandHolder interrupterHolder = new MockCommandHolder(true, hasDefaultCommand);
+      Command interrupter = interrupterHolder.getMock();
 
-    MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
-    Command defaultCommand = defaultHolder.getMock();
-    MockCommandHolder interrupterHolder = new MockCommandHolder(true, hasDefaultCommand);
-    Command interrupter = interrupterHolder.getMock();
+      scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+      scheduler.run();
+      scheduler.schedule(interrupter);
 
-    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
-    scheduler.run();
-    scheduler.schedule(interrupter);
+      assertFalse(scheduler.isScheduled(defaultCommand));
+      assertTrue(scheduler.isScheduled(interrupter));
 
-    assertFalse(scheduler.isScheduled(defaultCommand));
-    assertTrue(scheduler.isScheduled(interrupter));
+      scheduler.cancel(interrupter);
+      scheduler.run();
 
-    scheduler.cancel(interrupter);
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(defaultCommand));
-    assertFalse(scheduler.isScheduled(interrupter));
+      assertTrue(scheduler.isScheduled(defaultCommand));
+      assertFalse(scheduler.isScheduled(interrupter));
+    }
   }
 
   @Test
   void defaultCommandDisableResumeTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      TestSubsystem hasDefaultCommand = new TestSubsystem();
 
-    TestSubsystem hasDefaultCommand = new TestSubsystem();
+      MockCommandHolder defaultHolder = new MockCommandHolder(false, hasDefaultCommand);
+      Command defaultCommand = defaultHolder.getMock();
 
-    MockCommandHolder defaultHolder = new MockCommandHolder(false, hasDefaultCommand);
-    Command defaultCommand = defaultHolder.getMock();
+      scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+      scheduler.run();
 
-    scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
-    scheduler.run();
+      assertTrue(scheduler.isScheduled(defaultCommand));
 
-    assertTrue(scheduler.isScheduled(defaultCommand));
+      setDSEnabled(false);
+      scheduler.run();
 
-    setDSEnabled(false);
-    scheduler.run();
+      assertFalse(scheduler.isScheduled(defaultCommand));
 
-    assertFalse(scheduler.isScheduled(defaultCommand));
+      setDSEnabled(true);
+      scheduler.run();
 
-    setDSEnabled(true);
-    scheduler.run();
+      assertTrue(scheduler.isScheduled(defaultCommand));
 
-    assertTrue(scheduler.isScheduled(defaultCommand));
-
-    verify(defaultCommand).end(true);
+      verify(defaultCommand).end(true);
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
index 23f508a..484cdf9 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,29 +15,29 @@
 class FunctionalCommandTest extends CommandTestBase {
   @Test
   void functionalCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder cond1 = new ConditionHolder();
+      ConditionHolder cond2 = new ConditionHolder();
+      ConditionHolder cond3 = new ConditionHolder();
+      ConditionHolder cond4 = new ConditionHolder();
 
-    ConditionHolder cond1 = new ConditionHolder();
-    ConditionHolder cond2 = new ConditionHolder();
-    ConditionHolder cond3 = new ConditionHolder();
-    ConditionHolder cond4 = new ConditionHolder();
+      FunctionalCommand command =
+          new FunctionalCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true),
+              interrupted -> cond3.setCondition(true), cond4::getCondition);
 
-    FunctionalCommand command =
-        new FunctionalCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true),
-            interrupted -> cond3.setCondition(true), cond4::getCondition);
+      scheduler.schedule(command);
+      scheduler.run();
 
-    scheduler.schedule(command);
-    scheduler.run();
+      assertTrue(scheduler.isScheduled(command));
 
-    assertTrue(scheduler.isScheduled(command));
+      cond4.setCondition(true);
 
-    cond4.setCondition(true);
+      scheduler.run();
 
-    scheduler.run();
-
-    assertFalse(scheduler.isScheduled(command));
-    assertTrue(cond1.getCondition());
-    assertTrue(cond2.getCondition());
-    assertTrue(cond3.getCondition());
+      assertFalse(scheduler.isScheduled(command));
+      assertTrue(cond1.getCondition());
+      assertTrue(cond2.getCondition());
+      assertTrue(cond3.getCondition());
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
index a109ed6..e2b9dbe 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,16 +15,16 @@
 class InstantCommandTest extends CommandTestBase {
   @Test
   void instantCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder cond = new ConditionHolder();
 
-    ConditionHolder cond = new ConditionHolder();
+      InstantCommand command = new InstantCommand(() -> cond.setCondition(true));
 
-    InstantCommand command = new InstantCommand(() -> cond.setCondition(true));
+      scheduler.schedule(command);
+      scheduler.run();
 
-    scheduler.schedule(command);
-    scheduler.run();
-
-    assertTrue(cond.getCondition());
-    assertFalse(scheduler.isScheduled(command));
+      assertTrue(cond.getCondition());
+      assertFalse(scheduler.isScheduled(command));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
index 2ac2129..9d9ebfd 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,8 +9,12 @@
 
 import java.util.ArrayList;
 
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
@@ -20,6 +24,7 @@
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
@@ -28,6 +33,17 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 class MecanumControllerCommandTest {
+  @BeforeEach
+  void setupAll() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanupAll() {
+    SimHooks.resumeTiming();
+  }
+
   private final Timer m_timer = new Timer();
   private Rotation2d m_angle = new Rotation2d(0);
 
@@ -73,6 +89,7 @@
   }
 
   @Test
+  @ResourceLock("timing")
   @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
     final var subsystem = new Subsystem() {};
@@ -102,15 +119,16 @@
     while (!command.isFinished()) {
       command.execute();
       m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+      SimHooks.stepTiming(0.005);
     }
     m_timer.stop();
     command.end(true);
 
     assertAll(
-        () -> assertEquals(endState.poseMeters.getTranslation().getX(),
-          getRobotPose().getTranslation().getX(), kxTolerance),
-        () -> assertEquals(endState.poseMeters.getTranslation().getY(),
-          getRobotPose().getTranslation().getY(), kyTolerance),
+        () -> assertEquals(endState.poseMeters.getX(),
+          getRobotPose().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getY(),
+          getRobotPose().getY(), kyTolerance),
         () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
           getRobotPose().getRotation().getRadians(), kAngularTolerance)
     );

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
index fb06844..2448821 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,29 +7,43 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.condition.DisabledOnOs;
-import org.junit.jupiter.api.condition.OS;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
-import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 
-@DisabledOnOs(OS.MAC)
 class NotifierCommandTest extends CommandTestBase {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
   @Test
+  @ResourceLock("timing")
   void notifierCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Counter counter = new Counter();
 
-    Counter counter = new Counter();
+      NotifierCommand command = new NotifierCommand(counter::increment, 0.01);
 
-    NotifierCommand command = new NotifierCommand(counter::increment, 0.01);
+      scheduler.schedule(command);
+      for (int i = 0; i < 5; ++i) {
+        SimHooks.stepTiming(0.005);
+      }
+      scheduler.cancel(command);
 
-    scheduler.schedule(command);
-    Timer.delay(0.25);
-    scheduler.cancel(command);
-
-    assertTrue(counter.m_counter > 10, "Should have hit at least 10 triggers");
-    assertTrue(counter.m_counter < 100, "Shouldn't hit more then 100 triggers");
+      assertEquals(2, counter.m_counter);
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
index d03b4aa..a32b994 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,73 +20,74 @@
 class ParallelCommandGroupTest extends CommandTestBase {
   @Test
   void parallelGroupScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      Command group = new ParallelCommandGroup(command1, command2);
 
-    Command group = new ParallelCommandGroup(command1, command2);
+      scheduler.schedule(group);
 
-    scheduler.schedule(group);
+      verify(command1).initialize();
+      verify(command2).initialize();
 
-    verify(command1).initialize();
-    verify(command2).initialize();
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      scheduler.run();
 
-    command1Holder.setFinished(true);
-    scheduler.run();
-    command2Holder.setFinished(true);
-    scheduler.run();
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2, times(2)).execute();
+      verify(command2).end(false);
 
-    verify(command1).execute();
-    verify(command1).end(false);
-    verify(command2, times(2)).execute();
-    verify(command2).end(false);
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void parallelGroupInterruptTest() {
-    CommandScheduler scheduler = new CommandScheduler();
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    Command group = new ParallelCommandGroup(command1, command2);
+      Command group = new ParallelCommandGroup(command1, command2);
 
-    scheduler.schedule(group);
+      scheduler.schedule(group);
 
-    command1Holder.setFinished(true);
-    scheduler.run();
-    scheduler.run();
-    scheduler.cancel(group);
+      command1Holder.setFinished(true);
+      scheduler.run();
+      scheduler.run();
+      scheduler.cancel(group);
 
-    verify(command1).execute();
-    verify(command1).end(false);
-    verify(command1, never()).end(true);
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command1, never()).end(true);
 
-    verify(command2, times(2)).execute();
-    verify(command2, never()).end(false);
-    verify(command2).end(true);
+      verify(command2, times(2)).execute();
+      verify(command2, never()).end(false);
+      verify(command2).end(true);
 
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void notScheduledCancelTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      Command group = new ParallelCommandGroup(command1, command2);
 
-    Command group = new ParallelCommandGroup(command1, command2);
-
-    assertDoesNotThrow(() -> scheduler.cancel(group));
+      assertDoesNotThrow(() -> scheduler.cancel(group));
+    }
   }
 
   @Test
@@ -96,22 +97,22 @@
     Subsystem system3 = new TestSubsystem();
     Subsystem system4 = new TestSubsystem();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
-    Command command3 = command3Holder.getMock();
+      Command group = new ParallelCommandGroup(command1, command2);
 
-    Command group = new ParallelCommandGroup(command1, command2);
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
 
-    scheduler.schedule(group);
-    scheduler.schedule(command3);
-
-    assertFalse(scheduler.isScheduled(group));
-    assertTrue(scheduler.isScheduled(command3));
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
   }
 
   @Test

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
index c7e3769..bc10a0e 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,71 +19,71 @@
 class ParallelDeadlineGroupTest extends CommandTestBase {
   @Test
   void parallelDeadlineScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      command2Holder.setFinished(true);
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    command2Holder.setFinished(true);
-    MockCommandHolder command3Holder = new MockCommandHolder(true);
-    Command command3 = command3Holder.getMock();
+      Command group = new ParallelDeadlineGroup(command1, command2, command3);
 
-    Command group = new ParallelDeadlineGroup(command1, command2, command3);
+      scheduler.schedule(group);
+      scheduler.run();
 
-    scheduler.schedule(group);
-    scheduler.run();
+      assertTrue(scheduler.isScheduled(group));
 
-    assertTrue(scheduler.isScheduled(group));
+      command1Holder.setFinished(true);
+      scheduler.run();
 
-    command1Holder.setFinished(true);
-    scheduler.run();
+      assertFalse(scheduler.isScheduled(group));
 
-    assertFalse(scheduler.isScheduled(group));
+      verify(command2).initialize();
+      verify(command2).execute();
+      verify(command2).end(false);
+      verify(command2, never()).end(true);
 
-    verify(command2).initialize();
-    verify(command2).execute();
-    verify(command2).end(false);
-    verify(command2, never()).end(true);
+      verify(command1).initialize();
+      verify(command1, times(2)).execute();
+      verify(command1).end(false);
+      verify(command1, never()).end(true);
 
-    verify(command1).initialize();
-    verify(command1, times(2)).execute();
-    verify(command1).end(false);
-    verify(command1, never()).end(true);
-
-    verify(command3).initialize();
-    verify(command3, times(2)).execute();
-    verify(command3, never()).end(false);
-    verify(command3).end(true);
+      verify(command3).initialize();
+      verify(command3, times(2)).execute();
+      verify(command3, never()).end(false);
+      verify(command3).end(true);
+    }
   }
 
   @Test
   void parallelDeadlineInterruptTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      command2Holder.setFinished(true);
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    command2Holder.setFinished(true);
+      Command group = new ParallelDeadlineGroup(command1, command2);
 
-    Command group = new ParallelDeadlineGroup(command1, command2);
+      scheduler.schedule(group);
 
-    scheduler.schedule(group);
+      scheduler.run();
+      scheduler.run();
+      scheduler.cancel(group);
 
-    scheduler.run();
-    scheduler.run();
-    scheduler.cancel(group);
+      verify(command1, times(2)).execute();
+      verify(command1, never()).end(false);
+      verify(command1).end(true);
 
-    verify(command1, times(2)).execute();
-    verify(command1, never()).end(false);
-    verify(command1).end(true);
+      verify(command2).execute();
+      verify(command2).end(false);
+      verify(command2, never()).end(true);
 
-    verify(command2).execute();
-    verify(command2).end(false);
-    verify(command2, never()).end(true);
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
 
@@ -94,22 +94,22 @@
     Subsystem system3 = new TestSubsystem();
     Subsystem system4 = new TestSubsystem();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
-    Command command3 = command3Holder.getMock();
+      Command group = new ParallelDeadlineGroup(command1, command2);
 
-    Command group = new ParallelDeadlineGroup(command1, command2);
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
 
-    scheduler.schedule(group);
-    scheduler.schedule(command3);
-
-    assertFalse(scheduler.isScheduled(group));
-    assertTrue(scheduler.isScheduled(command3));
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
   }
 
   @Test

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
index 8a4781f..360882d 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,80 +15,81 @@
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.reset;
 import static org.mockito.Mockito.times;
 import static org.mockito.Mockito.verify;
 
 class ParallelRaceGroupTest extends CommandTestBase {
   @Test
   void parallelRaceScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      Command group = new ParallelRaceGroup(command1, command2);
 
-    Command group = new ParallelRaceGroup(command1, command2);
+      scheduler.schedule(group);
 
-    scheduler.schedule(group);
+      verify(command1).initialize();
+      verify(command2).initialize();
 
-    verify(command1).initialize();
-    verify(command2).initialize();
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      scheduler.run();
 
-    command1Holder.setFinished(true);
-    scheduler.run();
-    command2Holder.setFinished(true);
-    scheduler.run();
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).execute();
+      verify(command2).end(true);
+      verify(command2, never()).end(false);
 
-    verify(command1).execute();
-    verify(command1).end(false);
-    verify(command2).execute();
-    verify(command2).end(true);
-    verify(command2, never()).end(false);
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void parallelRaceInterruptTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      Command group = new ParallelRaceGroup(command1, command2);
 
-    Command group = new ParallelRaceGroup(command1, command2);
+      scheduler.schedule(group);
 
-    scheduler.schedule(group);
+      scheduler.run();
+      scheduler.run();
+      scheduler.cancel(group);
 
-    scheduler.run();
-    scheduler.run();
-    scheduler.cancel(group);
+      verify(command1, times(2)).execute();
+      verify(command1, never()).end(false);
+      verify(command1).end(true);
 
-    verify(command1, times(2)).execute();
-    verify(command1, never()).end(false);
-    verify(command1).end(true);
+      verify(command2, times(2)).execute();
+      verify(command2, never()).end(false);
+      verify(command2).end(true);
 
-    verify(command2, times(2)).execute();
-    verify(command2, never()).end(false);
-    verify(command2).end(true);
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void notScheduledCancelTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      Command group = new ParallelRaceGroup(command1, command2);
 
-    Command group = new ParallelRaceGroup(command1, command2);
-
-    assertDoesNotThrow(() -> scheduler.cancel(group));
+      assertDoesNotThrow(() -> scheduler.cancel(group));
+    }
   }
 
 
@@ -99,22 +100,22 @@
     Subsystem system3 = new TestSubsystem();
     Subsystem system4 = new TestSubsystem();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
-    Command command3 = command3Holder.getMock();
+      Command group = new ParallelRaceGroup(command1, command2);
 
-    Command group = new ParallelRaceGroup(command1, command2);
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
 
-    scheduler.schedule(group);
-    scheduler.schedule(command3);
-
-    assertFalse(scheduler.isScheduled(group));
-    assertTrue(scheduler.isScheduled(command3));
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
   }
 
   @Test
@@ -148,16 +149,61 @@
     assertNotNull(command3);
     Command group2 = new ParallelRaceGroup(group1, command3);
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      scheduler.schedule(group2);
+      scheduler.run();
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      // at this point the sequential group should be done
+      assertDoesNotThrow(() -> scheduler.run());
+    }
+  }
 
-    scheduler.schedule(group2);
-    scheduler.run();
-    command1Holder.setFinished(true);
-    scheduler.run();
-    command2Holder.setFinished(true);
-    // at this point the sequential group should be done
-    assertDoesNotThrow(() -> scheduler.run());
+  @Test
+  void parallelRaceScheduleTwiceTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
+      Command group = new ParallelRaceGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      verify(command1).initialize();
+      verify(command2).initialize();
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      scheduler.run();
+
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).execute();
+      verify(command2).end(true);
+      verify(command2, never()).end(false);
+
+      assertFalse(scheduler.isScheduled(group));
+
+      reset(command1);
+      reset(command2);
+
+      scheduler.schedule(group);
+
+      verify(command1).initialize();
+      verify(command2).initialize();
+
+      scheduler.run();
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(group));
+      command2Holder.setFinished(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
index baf037f..58195f3 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,13 +14,13 @@
 class PerpetualCommandTest extends CommandTestBase {
   @Test
   void perpetualCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      PerpetualCommand command = new PerpetualCommand(new InstantCommand());
 
-    PerpetualCommand command = new PerpetualCommand(new InstantCommand());
+      scheduler.schedule(command);
+      scheduler.run();
 
-    scheduler.schedule(command);
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(command));
+      assertTrue(scheduler.isScheduled(command));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
index 7484396..1895fc0 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,20 +18,21 @@
 class PrintCommandTest extends CommandTestBase {
   @Test
   void printCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      final PrintStream originalOut = System.out;
+      ByteArrayOutputStream testOut = new ByteArrayOutputStream();
+      System.setOut(new PrintStream(testOut));
+      try {
+        PrintCommand command = new PrintCommand("Test!");
 
-    final PrintStream originalOut = System.out;
-    ByteArrayOutputStream testOut = new ByteArrayOutputStream();
-    System.setOut(new PrintStream(testOut));
+        scheduler.schedule(command);
+        scheduler.run();
 
-    PrintCommand command = new PrintCommand("Test!");
-
-    scheduler.schedule(command);
-    scheduler.run();
-
-    assertFalse(scheduler.isScheduled(command));
-    assertEquals(testOut.toString(), "Test!" + System.lineSeparator());
-
-    System.setOut(originalOut);
+        assertFalse(scheduler.isScheduled(command));
+        assertEquals(testOut.toString(), "Test!" + System.lineSeparator());
+      } finally {
+        System.setOut(originalOut);
+      }
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
index b566aae..5364b95 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,36 +16,36 @@
 class ProxyScheduleCommandTest extends CommandTestBase {
   @Test
   void proxyScheduleCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
+      ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command1);
 
-    ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command1);
+      scheduler.schedule(scheduleCommand);
 
-    scheduler.schedule(scheduleCommand);
-
-    verify(command1).schedule();
+      verify(command1).schedule();
+    }
   }
 
   @Test
   void proxyScheduleCommandEndTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
+    try (CommandScheduler scheduler = CommandScheduler.getInstance()) {
+      ConditionHolder cond = new ConditionHolder();
 
-    ConditionHolder cond = new ConditionHolder();
+      WaitUntilCommand command = new WaitUntilCommand(cond::getCondition);
 
-    WaitUntilCommand command = new WaitUntilCommand(cond::getCondition);
+      ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command);
 
-    ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command);
+      scheduler.schedule(scheduleCommand);
 
-    scheduler.schedule(scheduleCommand);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(scheduleCommand));
 
-    scheduler.run();
-    assertTrue(scheduler.isScheduled(scheduleCommand));
-
-    cond.setCondition(true);
-    scheduler.run();
-    scheduler.run();
-    assertFalse(scheduler.isScheduled(scheduleCommand));
+      cond.setCondition(true);
+      scheduler.run();
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(scheduleCommand));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
index e862216..3b607bf 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,94 +18,94 @@
 class RobotDisabledCommandTest extends CommandTestBase {
   @Test
   void robotDisabledCommandCancelTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(false);
+      Command mockCommand = holder.getMock();
 
-    MockCommandHolder holder = new MockCommandHolder(false);
-    Command mockCommand = holder.getMock();
+      scheduler.schedule(mockCommand);
 
-    scheduler.schedule(mockCommand);
+      assertTrue(scheduler.isScheduled(mockCommand));
 
-    assertTrue(scheduler.isScheduled(mockCommand));
+      setDSEnabled(false);
 
-    setDSEnabled(false);
+      scheduler.run();
 
-    scheduler.run();
+      assertFalse(scheduler.isScheduled(mockCommand));
 
-    assertFalse(scheduler.isScheduled(mockCommand));
-
-    setDSEnabled(true);
+      setDSEnabled(true);
+    }
   }
 
   @Test
   void runWhenDisabledTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
 
-    MockCommandHolder holder = new MockCommandHolder(true);
-    Command mockCommand = holder.getMock();
+      scheduler.schedule(mockCommand);
 
-    scheduler.schedule(mockCommand);
+      assertTrue(scheduler.isScheduled(mockCommand));
 
-    assertTrue(scheduler.isScheduled(mockCommand));
+      setDSEnabled(false);
 
-    setDSEnabled(false);
+      scheduler.run();
 
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(mockCommand));
+      assertTrue(scheduler.isScheduled(mockCommand));
+    }
   }
 
   @Test
   void sequentialGroupRunWhenDisabledTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+      MockCommandHolder command4Holder = new MockCommandHolder(false);
+      Command command4 = command4Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true);
-    Command command3 = command3Holder.getMock();
-    MockCommandHolder command4Holder = new MockCommandHolder(false);
-    Command command4 = command4Holder.getMock();
+      Command runWhenDisabled = new SequentialCommandGroup(command1, command2);
+      Command dontRunWhenDisabled = new SequentialCommandGroup(command3, command4);
 
-    Command runWhenDisabled = new SequentialCommandGroup(command1, command2);
-    Command dontRunWhenDisabled = new SequentialCommandGroup(command3, command4);
+      scheduler.schedule(runWhenDisabled);
+      scheduler.schedule(dontRunWhenDisabled);
 
-    scheduler.schedule(runWhenDisabled);
-    scheduler.schedule(dontRunWhenDisabled);
+      setDSEnabled(false);
 
-    setDSEnabled(false);
+      scheduler.run();
 
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(runWhenDisabled));
-    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
   }
 
   @Test
   void parallelGroupRunWhenDisabledTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+      MockCommandHolder command4Holder = new MockCommandHolder(false);
+      Command command4 = command4Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true);
-    Command command3 = command3Holder.getMock();
-    MockCommandHolder command4Holder = new MockCommandHolder(false);
-    Command command4 = command4Holder.getMock();
+      Command runWhenDisabled = new ParallelCommandGroup(command1, command2);
+      Command dontRunWhenDisabled = new ParallelCommandGroup(command3, command4);
 
-    Command runWhenDisabled = new ParallelCommandGroup(command1, command2);
-    Command dontRunWhenDisabled = new ParallelCommandGroup(command3, command4);
+      scheduler.schedule(runWhenDisabled);
+      scheduler.schedule(dontRunWhenDisabled);
 
-    scheduler.schedule(runWhenDisabled);
-    scheduler.schedule(dontRunWhenDisabled);
+      setDSEnabled(false);
 
-    setDSEnabled(false);
+      scheduler.run();
 
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(runWhenDisabled));
-    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
   }
 
   @Test
@@ -121,15 +121,15 @@
     MockCommandHolder command4Holder = new MockCommandHolder(false);
     Command command4 = command4Holder.getMock();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+      Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
 
-    Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
-    Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+      scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
 
-    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
-
-    assertTrue(scheduler.isScheduled(runWhenDisabled));
-    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
   }
 
   @Test
@@ -148,12 +148,12 @@
     Command runWhenDisabled = new SelectCommand(Map.of(1, command1, 2, command2), () -> 1);
     Command dontRunWhenDisabled = new SelectCommand(Map.of(1, command3, 2, command4), () -> 1);
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
 
-    scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
-
-    assertTrue(scheduler.isScheduled(runWhenDisabled));
-    assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
   }
 
   @Test
@@ -169,15 +169,15 @@
     MockCommandHolder command4Holder = new MockCommandHolder(false);
     Command command4 = command4Holder.getMock();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+      Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
 
-    Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
-    Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+      Command parallel = parallel(runWhenDisabled, dontRunWhenDisabled);
 
-    Command parallel = parallel(runWhenDisabled, dontRunWhenDisabled);
+      scheduler.schedule(parallel);
 
-    scheduler.schedule(parallel);
-
-    assertFalse(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(runWhenDisabled));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
index 3ff8c3d..07c18dd 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,17 +14,17 @@
 class RunCommandTest extends CommandTestBase {
   @Test
   void runCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Counter counter = new Counter();
 
-    Counter counter = new Counter();
+      RunCommand command = new RunCommand(counter::increment);
 
-    RunCommand command = new RunCommand(counter::increment);
+      scheduler.schedule(command);
+      scheduler.run();
+      scheduler.run();
+      scheduler.run();
 
-    scheduler.schedule(command);
-    scheduler.run();
-    scheduler.run();
-    scheduler.run();
-
-    assertEquals(3, counter.m_counter);
+      assertEquals(3, counter.m_counter);
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
index 9041627..c8d5127 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,33 +15,33 @@
 class ScheduleCommandTest extends CommandTestBase {
   @Test
   void scheduleCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      ScheduleCommand scheduleCommand = new ScheduleCommand(command1, command2);
 
-    ScheduleCommand scheduleCommand = new ScheduleCommand(command1, command2);
+      scheduler.schedule(scheduleCommand);
 
-    scheduler.schedule(scheduleCommand);
-
-    verify(command1).schedule();
-    verify(command2).schedule();
+      verify(command1).schedule();
+      verify(command2).schedule();
+    }
   }
 
   @Test
   void scheduleCommandDuringRunTest() {
-    CommandScheduler scheduler = CommandScheduler.getInstance();
+    try (CommandScheduler scheduler = CommandScheduler.getInstance()) {
+      InstantCommand toSchedule = new InstantCommand();
+      ScheduleCommand scheduleCommand = new ScheduleCommand(toSchedule);
+      SequentialCommandGroup group =
+          new SequentialCommandGroup(new InstantCommand(), scheduleCommand);
 
-    InstantCommand toSchedule = new InstantCommand();
-    ScheduleCommand scheduleCommand = new ScheduleCommand(toSchedule);
-    SequentialCommandGroup group =
-        new SequentialCommandGroup(new InstantCommand(), scheduleCommand);
-
-    scheduler.schedule(group);
-    scheduler.schedule(new InstantCommand().perpetually());
-    scheduler.run();
-    assertDoesNotThrow(scheduler::run);
+      scheduler.schedule(group);
+      scheduler.schedule(new InstantCommand().perpetually());
+      scheduler.run();
+      assertDoesNotThrow(scheduler::run);
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
index 1f110b6..c148816 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,43 +15,61 @@
 class SchedulerTest extends CommandTestBase {
   @Test
   void schedulerLambdaTestNoInterrupt() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Counter counter = new Counter();
 
-    Counter counter = new Counter();
+      scheduler.onCommandInitialize(command -> counter.increment());
+      scheduler.onCommandExecute(command -> counter.increment());
+      scheduler.onCommandFinish(command -> counter.increment());
 
-    scheduler.onCommandInitialize(command -> counter.increment());
-    scheduler.onCommandExecute(command -> counter.increment());
-    scheduler.onCommandFinish(command -> counter.increment());
+      scheduler.schedule(new InstantCommand());
+      scheduler.run();
 
-    scheduler.schedule(new InstantCommand());
-    scheduler.run();
-
-    assertEquals(counter.m_counter, 3);
+      assertEquals(counter.m_counter, 3);
+    }
   }
 
   @Test
   void schedulerInterruptLambdaTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Counter counter = new Counter();
 
-    Counter counter = new Counter();
+      scheduler.onCommandInterrupt(command -> counter.increment());
 
-    scheduler.onCommandInterrupt(command -> counter.increment());
+      Command command = new WaitCommand(10);
 
-    Command command = new WaitCommand(10);
+      scheduler.schedule(command);
+      scheduler.cancel(command);
 
-    scheduler.schedule(command);
-    scheduler.cancel(command);
-
-    assertEquals(counter.m_counter, 1);
+      assertEquals(counter.m_counter, 1);
+    }
   }
 
   @Test
   void unregisterSubsystemTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem system = new TestSubsystem();
 
-    Subsystem system = new TestSubsystem();
+      scheduler.registerSubsystem(system);
+      assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system));
+    }
+  }
 
-    scheduler.registerSubsystem(system);
-    assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system));
+  @Test
+  void schedulerCancelAllTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Counter counter = new Counter();
+
+      scheduler.onCommandInterrupt(command -> counter.increment());
+
+      Command command = new WaitCommand(10);
+      Command command2 = new WaitCommand(10);
+
+      scheduler.schedule(command);
+      scheduler.schedule(command2);
+      scheduler.cancelAll();
+
+      assertEquals(counter.m_counter, 2);
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
index df52855..53f6a98 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,59 +19,59 @@
 class SelectCommandTest extends CommandTestBase {
   @Test
   void selectCommandTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      command1Holder.setFinished(true);
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    command1Holder.setFinished(true);
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true);
-    Command command3 = command3Holder.getMock();
+      SelectCommand selectCommand =
+          new SelectCommand(Map.ofEntries(
+              Map.entry("one", command1),
+              Map.entry("two", command2),
+              Map.entry("three", command3)),
+              () -> "one");
 
-    SelectCommand selectCommand =
-        new SelectCommand(Map.ofEntries(
-            Map.entry("one", command1),
-            Map.entry("two", command2),
-            Map.entry("three", command3)),
-            () -> "one");
+      scheduler.schedule(selectCommand);
+      scheduler.run();
 
-    scheduler.schedule(selectCommand);
-    scheduler.run();
+      verify(command1).initialize();
+      verify(command1).execute();
+      verify(command1).end(false);
 
-    verify(command1).initialize();
-    verify(command1).execute();
-    verify(command1).end(false);
+      verify(command2, never()).initialize();
+      verify(command2, never()).execute();
+      verify(command2, never()).end(false);
 
-    verify(command2, never()).initialize();
-    verify(command2, never()).execute();
-    verify(command2, never()).end(false);
-
-    verify(command3, never()).initialize();
-    verify(command3, never()).execute();
-    verify(command3, never()).end(false);
+      verify(command3, never()).initialize();
+      verify(command3, never()).execute();
+      verify(command3, never()).end(false);
+    }
   }
 
   @Test
   void selectCommandInvalidKeyTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      command1Holder.setFinished(true);
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    command1Holder.setFinished(true);
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true);
-    Command command3 = command3Holder.getMock();
+      SelectCommand selectCommand =
+          new SelectCommand(Map.ofEntries(
+              Map.entry("one", command1),
+              Map.entry("two", command2),
+              Map.entry("three", command3)),
+              () -> "four");
 
-    SelectCommand selectCommand =
-        new SelectCommand(Map.ofEntries(
-            Map.entry("one", command1),
-            Map.entry("two", command2),
-            Map.entry("three", command3)),
-            () -> "four");
-
-    assertDoesNotThrow(() -> scheduler.schedule(selectCommand));
+      assertDoesNotThrow(() -> scheduler.schedule(selectCommand));
+    }
   }
 
 
@@ -82,27 +82,27 @@
     Subsystem system3 = new TestSubsystem();
     Subsystem system4 = new TestSubsystem();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
-    Command command3 = command3Holder.getMock();
+      SelectCommand selectCommand = new SelectCommand(
+          Map.ofEntries(Map.entry("one", command1), Map.entry("two", command2),
+              Map.entry("three", command3)), () -> "one");
 
-    SelectCommand selectCommand = new SelectCommand(
-        Map.ofEntries(Map.entry("one", command1), Map.entry("two", command2),
-            Map.entry("three", command3)), () -> "one");
+      scheduler.schedule(selectCommand);
+      scheduler.schedule(new InstantCommand(() -> {
+      }, system3));
 
-    scheduler.schedule(selectCommand);
-    scheduler.schedule(new InstantCommand(() -> {
-    }, system3));
+      assertFalse(scheduler.isScheduled(selectCommand));
 
-    assertFalse(scheduler.isScheduled(selectCommand));
-
-    verify(command1).end(true);
-    verify(command2, never()).end(true);
-    verify(command3, never()).end(true);
+      verify(command1).end(true);
+      verify(command2, never()).end(true);
+      verify(command3, never()).end(true);
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
index 8582140..a47f865 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,86 +18,86 @@
 class SequentialCommandGroupTest extends CommandTestBase {
   @Test
   void sequentialGroupScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      Command group = new SequentialCommandGroup(command1, command2);
 
-    Command group = new SequentialCommandGroup(command1, command2);
+      scheduler.schedule(group);
 
-    scheduler.schedule(group);
+      verify(command1).initialize();
+      verify(command2, never()).initialize();
 
-    verify(command1).initialize();
-    verify(command2, never()).initialize();
+      command1Holder.setFinished(true);
+      scheduler.run();
 
-    command1Holder.setFinished(true);
-    scheduler.run();
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).initialize();
+      verify(command2, never()).execute();
+      verify(command2, never()).end(false);
 
-    verify(command1).execute();
-    verify(command1).end(false);
-    verify(command2).initialize();
-    verify(command2, never()).execute();
-    verify(command2, never()).end(false);
+      command2Holder.setFinished(true);
+      scheduler.run();
 
-    command2Holder.setFinished(true);
-    scheduler.run();
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).execute();
+      verify(command2).end(false);
 
-    verify(command1).execute();
-    verify(command1).end(false);
-    verify(command2).execute();
-    verify(command2).end(false);
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void sequentialGroupInterruptTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true);
-    Command command3 = command3Holder.getMock();
+      Command group = new SequentialCommandGroup(command1, command2, command3);
 
-    Command group = new SequentialCommandGroup(command1, command2, command3);
+      scheduler.schedule(group);
 
-    scheduler.schedule(group);
+      command1Holder.setFinished(true);
+      scheduler.run();
+      scheduler.cancel(group);
+      scheduler.run();
 
-    command1Holder.setFinished(true);
-    scheduler.run();
-    scheduler.cancel(group);
-    scheduler.run();
+      verify(command1).execute();
+      verify(command1, never()).end(true);
+      verify(command1).end(false);
+      verify(command2, never()).execute();
+      verify(command2).end(true);
+      verify(command2, never()).end(false);
+      verify(command3, never()).initialize();
+      verify(command3, never()).execute();
+      verify(command3, never()).end(true);
+      verify(command3, never()).end(false);
 
-    verify(command1).execute();
-    verify(command1, never()).end(true);
-    verify(command1).end(false);
-    verify(command2, never()).execute();
-    verify(command2).end(true);
-    verify(command2, never()).end(false);
-    verify(command3, never()).initialize();
-    verify(command3, never()).execute();
-    verify(command3, never()).end(true);
-    verify(command3, never()).end(false);
-
-    assertFalse(scheduler.isScheduled(group));
+      assertFalse(scheduler.isScheduled(group));
+    }
   }
 
   @Test
   void notScheduledCancelTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true);
-    Command command2 = command2Holder.getMock();
+      Command group = new SequentialCommandGroup(command1, command2);
 
-    Command group = new SequentialCommandGroup(command1, command2);
-
-    assertDoesNotThrow(() -> scheduler.cancel(group));
+      assertDoesNotThrow(() -> scheduler.cancel(group));
+    }
   }
 
 
@@ -108,21 +108,21 @@
     Subsystem system3 = new TestSubsystem();
     Subsystem system4 = new TestSubsystem();
 
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
-    Command command1 = command1Holder.getMock();
-    MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
-    Command command2 = command2Holder.getMock();
-    MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
-    Command command3 = command3Holder.getMock();
+      Command group = new SequentialCommandGroup(command1, command2);
 
-    Command group = new SequentialCommandGroup(command1, command2);
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
 
-    scheduler.schedule(group);
-    scheduler.schedule(command3);
-
-    assertFalse(scheduler.isScheduled(group));
-    assertTrue(scheduler.isScheduled(command3));
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
index 93921e0..90ad41a 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,23 +15,23 @@
 class StartEndCommandTest extends CommandTestBase {
   @Test
   void startEndCommandScheduleTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder cond1 = new ConditionHolder();
+      ConditionHolder cond2 = new ConditionHolder();
 
-    ConditionHolder cond1 = new ConditionHolder();
-    ConditionHolder cond2 = new ConditionHolder();
+      StartEndCommand command =
+          new StartEndCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true));
 
-    StartEndCommand command =
-        new StartEndCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true));
+      scheduler.schedule(command);
+      scheduler.run();
 
-    scheduler.schedule(command);
-    scheduler.run();
+      assertTrue(scheduler.isScheduled(command));
 
-    assertTrue(scheduler.isScheduled(command));
+      scheduler.cancel(command);
 
-    scheduler.cancel(command);
-
-    assertFalse(scheduler.isScheduled(command));
-    assertTrue(cond1.getCondition());
-    assertTrue(cond2.getCondition());
+      assertFalse(scheduler.isScheduled(command));
+      assertTrue(cond1.getCondition());
+      assertTrue(cond2.getCondition());
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
index 5209199..2febfbf 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,8 +9,12 @@
 
 import java.util.ArrayList;
 
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
@@ -20,6 +24,7 @@
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
@@ -28,6 +33,17 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 class SwerveControllerCommandTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
   private final Timer m_timer = new Timer();
   private Rotation2d m_angle = new Rotation2d(0);
 
@@ -67,6 +83,7 @@
   }
 
   @Test
+  @ResourceLock("timing")
   @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
     final var subsystem = new Subsystem() {};
@@ -95,15 +112,16 @@
     while (!command.isFinished()) {
       command.execute();
       m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+      SimHooks.stepTiming(0.005);
     }
     m_timer.stop();
     command.end(true);
 
     assertAll(
-        () -> assertEquals(endState.poseMeters.getTranslation().getX(),
-          getRobotPose().getTranslation().getX(), kxTolerance),
-        () -> assertEquals(endState.poseMeters.getTranslation().getY(),
-          getRobotPose().getTranslation().getY(), kyTolerance),
+        () -> assertEquals(endState.poseMeters.getX(),
+          getRobotPose().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getY(),
+          getRobotPose().getY(), kyTolerance),
         () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
           getRobotPose().getRotation().getRadians(), kAngularTolerance)
     );

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
index 5a522af..f7f9296 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,9 +7,13 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -19,49 +23,62 @@
 import static org.mockito.Mockito.when;
 
 class WaitCommandTest extends CommandTestBase {
-  @Test
-  void waitCommandTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
 
-    WaitCommand waitCommand = new WaitCommand(2);
-
-    scheduler.schedule(waitCommand);
-    scheduler.run();
-    Timer.delay(1);
-    scheduler.run();
-
-    assertTrue(scheduler.isScheduled(waitCommand));
-
-    Timer.delay(2);
-
-    scheduler.run();
-
-    assertFalse(scheduler.isScheduled(waitCommand));
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
   }
 
   @Test
+  @ResourceLock("timing")
+  void waitCommandTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      WaitCommand waitCommand = new WaitCommand(2);
+
+      scheduler.schedule(waitCommand);
+      scheduler.run();
+      SimHooks.stepTiming(1);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(waitCommand));
+
+      SimHooks.stepTiming(2);
+
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(waitCommand));
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
   void withTimeoutTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      when(command1.withTimeout(anyDouble())).thenCallRealMethod();
 
-    MockCommandHolder command1Holder = new MockCommandHolder(true);
-    Command command1 = command1Holder.getMock();
-    when(command1.withTimeout(anyDouble())).thenCallRealMethod();
+      Command timeout = command1.withTimeout(2);
 
-    Command timeout = command1.withTimeout(2);
+      scheduler.schedule(timeout);
+      scheduler.run();
 
-    scheduler.schedule(timeout);
-    scheduler.run();
+      verify(command1).initialize();
+      verify(command1).execute();
+      assertFalse(scheduler.isScheduled(command1));
+      assertTrue(scheduler.isScheduled(timeout));
 
-    verify(command1).initialize();
-    verify(command1).execute();
-    assertFalse(scheduler.isScheduled(command1));
-    assertTrue(scheduler.isScheduled(timeout));
+      SimHooks.stepTiming(3);
+      scheduler.run();
 
-    Timer.delay(3);
-    scheduler.run();
-
-    verify(command1).end(true);
-    verify(command1, never()).end(false);
-    assertFalse(scheduler.isScheduled(timeout));
+      verify(command1).end(true);
+      verify(command1, never()).end(false);
+      assertFalse(scheduler.isScheduled(timeout));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
index a99a18c..c38feee 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,17 +15,17 @@
 class WaitUntilCommandTest extends CommandTestBase {
   @Test
   void waitUntilTest() {
-    CommandScheduler scheduler = new CommandScheduler();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      ConditionHolder condition = new ConditionHolder();
 
-    ConditionHolder condition = new ConditionHolder();
+      Command command = new WaitUntilCommand(condition::getCondition);
 
-    Command command = new WaitUntilCommand(condition::getCondition);
-
-    scheduler.schedule(command);
-    scheduler.run();
-    assertTrue(scheduler.isScheduled(command));
-    condition.setCondition(true);
-    scheduler.run();
-    assertFalse(scheduler.isScheduled(command));
+      scheduler.schedule(command);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command));
+      condition.setCondition(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(command));
+    }
   }
 }

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
new file mode 100644
index 0000000..44083d0
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
@@ -0,0 +1,181 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+
+class ButtonTest extends CommandTestBase {
+  @Test
+  void whenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+  }
+
+  @Test
+  void whenReleasedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(true);
+    button.whenReleased(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+  }
+
+  @Test
+  void whileHeldTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whileHeld(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1, times(2)).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void whenHeldTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whenHeld(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void toggleWhenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.toggleWhenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).schedule(true);
+    button.setPressed(true);
+    scheduler.run();
+    when(command1.isScheduled()).thenReturn(true);
+    scheduler.run();
+    verify(command1).schedule(true);
+    button.setPressed(false);
+    scheduler.run();
+    verify(command1, never()).cancel();
+    button.setPressed(true);
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void cancelWhenPressedTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.cancelWhenPressed(command1);
+    scheduler.run();
+    verify(command1, never()).cancel();
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command1).cancel();
+  }
+
+  @Test
+  void runnableBindingTest() {
+
+    InternalButton buttonWhenPressed = new InternalButton();
+    InternalButton buttonWhileHeld = new InternalButton();
+    InternalButton buttonWhenReleased = new InternalButton();
+
+    buttonWhenPressed.setPressed(false);
+    buttonWhileHeld.setPressed(true);
+    buttonWhenReleased.setPressed(true);
+
+    Counter counter = new Counter();
+
+    buttonWhenPressed.whenPressed(counter::increment);
+    buttonWhileHeld.whileHeld(counter::increment);
+    buttonWhenReleased.whenReleased(counter::increment);
+
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+
+    scheduler.run();
+    buttonWhenPressed.setPressed(true);
+    buttonWhenReleased.setPressed(false);
+    scheduler.run();
+
+    assertEquals(counter.m_counter, 4);
+  }
+
+  @Test
+  void buttonCompositionTest() {
+    InternalButton button1 = new InternalButton();
+    InternalButton button2 = new InternalButton();
+
+    button1.setPressed(true);
+    button2.setPressed(false);
+
+    assertFalse(button1.and(button2).get());
+    assertTrue(button1.or(button2).get());
+    assertFalse(button1.negate().get());
+    assertTrue(button1.and(button2.negate()).get());
+  }
+}

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java

diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
new file mode 100644
index 0000000..26a4a1c
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+class NetworkButtonTest extends CommandTestBase {
+  @BeforeEach
+  void setup() {
+    NetworkTableInstance.getDefault().startLocal();
+  }
+
+  @AfterEach
+  void teardown() {
+    NetworkTableInstance.getDefault().deleteAllEntries();
+    NetworkTableInstance.getDefault().stopLocal();
+  }
+
+  @Test
+  void setNetworkButtonTest() {
+    var scheduler = CommandScheduler.getInstance();
+    var commandHolder = new MockCommandHolder(true);
+    var command = commandHolder.getMock();
+    var entry = NetworkTableInstance.getDefault()
+        .getTable("TestTable")
+        .getEntry("Test");
+
+    var button = new NetworkButton("TestTable", "Test");
+    entry.setBoolean(false);
+    button.whenPressed(command);
+    scheduler.run();
+    verify(command, never()).schedule(true);
+    entry.setBoolean(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command).schedule(true);
+  }
+}

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
index 829f0d2..d504ff7 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -129,9 +129,9 @@
   bool pressed2 = false;
   WaitUntilCommand command([&finished] { return finished; });
 
-  (Trigger([&pressed1] { return pressed1; }) &&
-   Trigger([&pressed2] { return pressed2; }))
-      .WhenActive(&command);
+  (Trigger([&pressed1] { return pressed1; }) && Trigger([&pressed2] {
+     return pressed2;
+   })).WhenActive(&command);
   pressed1 = true;
   scheduler.Run();
   EXPECT_FALSE(scheduler.IsScheduled(&command));
@@ -148,18 +148,18 @@
   WaitUntilCommand command1([&finished] { return finished; });
   WaitUntilCommand command2([&finished] { return finished; });
 
-  (Trigger([&pressed1] { return pressed1; }) ||
-   Trigger([&pressed2] { return pressed2; }))
-      .WhenActive(&command1);
+  (Trigger([&pressed1] { return pressed1; }) || Trigger([&pressed2] {
+     return pressed2;
+   })).WhenActive(&command1);
   pressed1 = true;
   scheduler.Run();
   EXPECT_TRUE(scheduler.IsScheduled(&command1));
 
   pressed1 = false;
 
-  (Trigger([&pressed1] { return pressed1; }) ||
-   Trigger([&pressed2] { return pressed2; }))
-      .WhenActive(&command2);
+  (Trigger([&pressed1] { return pressed1; }) || Trigger([&pressed2] {
+     return pressed2;
+   })).WhenActive(&command2);
   pressed2 = true;
   scheduler.Run();
   EXPECT_TRUE(scheduler.IsScheduled(&command2));

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
index 5e876c4..67eb428 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
@@ -1,10 +1,12 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/simulation/SimHooks.h>
+
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelRaceGroup.h"
@@ -18,6 +20,8 @@
 TEST_F(CommandDecoratorTest, WithTimeoutTest) {
   CommandScheduler scheduler = GetScheduler();
 
+  frc::sim::PauseTiming();
+
   auto command = RunCommand([] {}, {}).WithTimeout(100_ms);
 
   scheduler.Schedule(&command);
@@ -25,10 +29,12 @@
   scheduler.Run();
   EXPECT_TRUE(scheduler.IsScheduled(&command));
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+  frc::sim::StepTiming(150_ms);
 
   scheduler.Run();
   EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  frc::sim::ResumeTiming();
 }
 
 TEST_F(CommandDecoratorTest, WithInterruptTest) {

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
index 0429c62..e5136ba 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,20 +18,12 @@
 
 CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
 
-void CommandTestBase::SetUp() {
-  HALSIM_SetDriverStationEnabled(true);
-  while (!HALSIM_GetDriverStationEnabled()) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-  }
-}
+void CommandTestBase::SetUp() { frc::sim::DriverStationSim::SetEnabled(true); }
 
 void CommandTestBase::TearDown() {
   CommandScheduler::GetInstance().ClearButtons();
 }
 
 void CommandTestBase::SetDSEnabled(bool enabled) {
-  HALSIM_SetDriverStationEnabled(enabled);
-  while (HALSIM_GetDriverStationEnabled() != static_cast<int>(enabled)) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-  }
+  frc::sim::DriverStationSim::SetEnabled(enabled);
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
index 8b22844..c6400a8 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,10 +8,9 @@
 #pragma once
 
 #include <memory>
-#include <regex>
 #include <utility>
 
-#include <mockdata/MockHooks.h>
+#include <frc/simulation/DriverStationSim.h>
 
 #include "ErrorConfirmer.h"
 #include "frc2/command/CommandGroupBase.h"
@@ -21,7 +20,6 @@
 #include "gmock/gmock.h"
 #include "gtest/gtest.h"
 #include "make_vector.h"
-#include "simulation/DriverStationSim.h"
 
 namespace frc2 {
 class CommandTestBase : public ::testing::Test {

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
index 2565a94..2c13f5d 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 #include "ErrorConfirmer.h"
 
+#include <regex>
+
 ErrorConfirmer* ErrorConfirmer::instance;
 
 int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,

wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
index 011158c..2df5e26 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,12 +7,9 @@
 
 #pragma once
 
-#include <regex>
-
-#include <mockdata/MockHooks.h>
+#include <hal/simulation/MockHooks.h>
 
 #include "gmock/gmock.h"
-#include "simulation/DriverStationSim.h"
 
 class ErrorConfirmer {
  public:

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
index 4eae880..07bc07a 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,14 +9,13 @@
 #include <frc2/command/MecanumControllerCommand.h>
 #include <frc2/command/Subsystem.h>
 
-#include <iostream>
-
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/MecanumDriveKinematics.h>
 #include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/simulation/SimHooks.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <wpi/math>
 
@@ -60,6 +59,10 @@
   frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
                                        frc::Pose2d{0_m, 0_m, 0_rad}};
 
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+
   frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
     return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
                                         m_rearLeftSpeed, m_rearRightSpeed};
@@ -72,7 +75,7 @@
 };
 
 TEST_F(MecanumControllerCommandTest, ReachesReference) {
-  frc2::Subsystem subsystem{};
+  frc2::Subsystem subsystem;
 
   auto waypoints =
       std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
@@ -99,18 +102,18 @@
 
   m_timer.Reset();
   m_timer.Start();
+
   command.Initialize();
   while (!command.IsFinished()) {
     command.Execute();
     m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+    frc::sim::StepTiming(5_ms);
   }
   m_timer.Stop();
   command.End(false);
 
-  EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
-                    getRobotPose().Translation().X(), kxTolerance);
-  EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
-                    getRobotPose().Translation().Y(), kyTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance);
   EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
                     getRobotPose().Rotation().Radians(), kAngularTolerance);
 }

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
index a34c292..b4df5a7 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
@@ -1,31 +1,35 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/simulation/SimHooks.h>
+
 #include "CommandTestBase.h"
 #include "frc2/command/NotifierCommand.h"
 
 using namespace frc2;
+using namespace std::chrono_literals;
+
 class NotifierCommandTest : public CommandTestBase {};
 
-#ifdef __APPLE__
-TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
-#else
 TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
-#endif
   CommandScheduler scheduler = GetScheduler();
 
-  int counter = 0;
+  frc::sim::PauseTiming();
 
-  NotifierCommand command([&counter] { counter++; }, 0.01_s, {});
+  int counter = 0;
+  NotifierCommand command([&] { counter++; }, 0.01_s, {});
 
   scheduler.Schedule(&command);
-  std::this_thread::sleep_for(std::chrono::milliseconds(250));
+  for (int i = 0; i < 5; ++i) {
+    frc::sim::StepTiming(0.005_s);
+  }
   scheduler.Cancel(&command);
 
-  EXPECT_GT(counter, 10);
-  EXPECT_LT(counter, 100);
+  frc::sim::ResumeTiming();
+
+  EXPECT_EQ(counter, 2);
 }

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp
new file mode 100644
index 0000000..2a1646d
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/Joystick.h>
+#include <frc/simulation/JoystickSim.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/POVButton.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class POVButtonTest : public CommandTestBase {};
+
+TEST_F(POVButtonTest, SetPOVTest) {
+  frc::sim::JoystickSim joysim(1);
+  joysim.SetPOV(0);
+  joysim.NotifyNewData();
+
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  frc::Joystick joy(1);
+  POVButton(&joy, 90).WhenPressed(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  joysim.SetPOV(90);
+  joysim.NotifyNewData();
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
index 54762ca..072bcb0 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -154,3 +154,54 @@
   EXPECT_NO_FATAL_FAILURE(scheduler.Run());
   EXPECT_FALSE(scheduler.IsScheduled(&group2));
 }
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwiceTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize()).Times(2);
+  EXPECT_CALL(*command1, Execute()).Times(5);
+  EXPECT_CALL(*command1, End(true)).Times(2);
+
+  EXPECT_CALL(*command2, Initialize()).Times(2);
+  EXPECT_CALL(*command2, Execute()).Times(5);
+  EXPECT_CALL(*command2, End(false)).Times(2);
+
+  EXPECT_CALL(*command3, Initialize()).Times(2);
+  EXPECT_CALL(*command3, Execute()).Times(5);
+  EXPECT_CALL(*command3, End(true)).Times(2);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(false);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
index f0198c9..2582ad9 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -54,3 +54,21 @@
 
   EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
 }
+
+TEST_F(SchedulerTest, SchedulerCancelAllTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  RunCommand command([] {}, {});
+  RunCommand command2([] {}, {});
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(&command);
+  scheduler.Schedule(&command2);
+  scheduler.Run();
+  scheduler.CancelAll();
+
+  EXPECT_EQ(counter, 2);
+}

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
index fa0838d..b2c47d0 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,8 +9,6 @@
 #include <frc2/command/Subsystem.h>
 #include <frc2/command/SwerveControllerCommand.h>
 
-#include <iostream>
-
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Rotation2d.h>
@@ -18,6 +16,7 @@
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/kinematics/SwerveDriveOdometry.h>
 #include <frc/kinematics/SwerveModuleState.h>
+#include <frc/simulation/SimHooks.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <wpi/math>
 
@@ -60,6 +59,10 @@
   frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad,
                                          frc::Pose2d{0_m, 0_m, 0_rad}};
 
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+
   std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
     return m_moduleStates;
   }
@@ -71,7 +74,7 @@
 };
 
 TEST_F(SwerveControllerCommandTest, ReachesReference) {
-  frc2::Subsystem subsystem{};
+  frc2::Subsystem subsystem;
 
   auto waypoints =
       std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
@@ -89,18 +92,18 @@
 
   m_timer.Reset();
   m_timer.Start();
+
   command.Initialize();
   while (!command.IsFinished()) {
     command.Execute();
     m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+    frc::sim::StepTiming(5_ms);
   }
   m_timer.Stop();
   command.End(false);
 
-  EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
-                    getRobotPose().Translation().X(), kxTolerance);
-  EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
-                    getRobotPose().Translation().Y(), kyTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance);
   EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
                     getRobotPose().Rotation().Radians(), kAngularTolerance);
 }

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

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
index 75841a6..000a9bb 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
@@ -1,10 +1,12 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/simulation/SimHooks.h>
+
 #include "CommandTestBase.h"
 #include "frc2/command/WaitCommand.h"
 #include "frc2/command/WaitUntilCommand.h"
@@ -13,6 +15,8 @@
 class WaitCommandTest : public CommandTestBase {};
 
 TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+  frc::sim::PauseTiming();
+
   CommandScheduler scheduler = GetScheduler();
 
   WaitCommand command(100_ms);
@@ -20,7 +24,9 @@
   scheduler.Schedule(&command);
   scheduler.Run();
   EXPECT_TRUE(scheduler.IsScheduled(&command));
-  std::this_thread::sleep_for(std::chrono::milliseconds(110));
+  frc::sim::StepTiming(110_ms);
   scheduler.Run();
   EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  frc::sim::ResumeTiming();
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp
new file mode 100644
index 0000000..18fef35
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <networktables/NetworkTableInstance.h>
+
+#include "../CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/NetworkButton.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+
+class NetworkButtonTest : public CommandTestBase {
+  void SetUp() override { nt::NetworkTableInstance::GetDefault().StartLocal(); }
+
+  void TearDown() override {
+    nt::NetworkTableInstance::GetDefault().DeleteAllEntries();
+    nt::NetworkTableInstance::GetDefault().StopLocal();
+  }
+};
+
+TEST_F(NetworkButtonTest, SetNetworkButtonTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  auto entry = nt::NetworkTableInstance::GetDefault()
+                   .GetTable("TestTable")
+                   ->GetEntry("Test");
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  NetworkButton("TestTable", "Test").WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  entry.SetBoolean(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

wpilibOldCommands/build.gradle

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

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java

diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
index da135ca..6233a43 100644
--- a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -40,7 +40,6 @@
  * @see CommandGroup
  * @see IllegalUseOfCommandException
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public abstract class Command implements Sendable, AutoCloseable {
   /**
    * The time since this command was initialized.
@@ -330,7 +329,7 @@
    * method for time-sensitive commands.
    *
    * <p>Returning false will result in the command never ending automatically. It may still be
-   * cancelled manually or interrupted by another command. Returning true will result in the
+   * canceled manually or interrupted by another command. Returning true will result in the
    * command executing once and finishing immediately. We recommend using {@link InstantCommand}
    * for this.
    *
@@ -447,7 +446,7 @@
 
   /**
    * Clears list of subsystem requirements. This is only used by
-   * {@link ConditionalCommand} so cancelling the chosen command works properly
+   * {@link ConditionalCommand} so canceling the chosen command works properly
    * in {@link CommandGroup}.
    */
   protected void clearRequirements() {

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java

diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
index 525e681..ef4f353 100644
--- a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -31,7 +31,6 @@
  * @see Subsystem
  * @see IllegalUseOfCommandException
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class CommandGroup extends Command {
   /**
    * The commands in this group (stored in entries).

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java

diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
index 2dc6f66..01d316e 100644
--- a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -141,7 +141,7 @@
 
     if (m_chosenCommand != null) {
       /*
-       * This is a hack to make cancelling the chosen command inside a
+       * This is a hack to make canceling the chosen command inside a
        * CommandGroup work properly
        */
       m_chosenCommand.clearRequirements();

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java

diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
index 1053f55..4ed7459 100644
--- a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,6 +18,7 @@
  * it is put into multiple command groups, it is started from outside its command group, or it adds
  * a new child. </p>
  */
+@SuppressWarnings("serial")
 public class IllegalUseOfCommandException extends RuntimeException {
   /**
    * Instantiates an {@link IllegalUseOfCommandException}.

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java

diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
index 1f30f38..7bb6331 100644
--- a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -66,7 +66,7 @@
   }
 
   /**
-   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values.
    *
    * @param name the name
    * @param p    the proportional value
@@ -82,7 +82,7 @@
   }
 
   /**
-   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will also
+   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will also
    * space the time between PID loop calculations to be equal to the given period.
    *
    * @param name   the name
@@ -114,9 +114,9 @@
   }
 
   /**
-   * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will use the
-   * class name as its name. It will also space the time between PID loop calculations to be equal
-   * to the given period.
+   * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will use
+   * the class name as its name. It will also space the time between PID loop calculations to be
+   * equal to the given period.
    *
    * @param p      the proportional value
    * @param i      the integral value

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java

diff --git a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
index e25a5a7..c48f604 100644
--- a/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
+++ b/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,6 +17,7 @@
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -32,7 +33,6 @@
  *
  * @see Command
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public final class Scheduler implements Sendable, AutoCloseable {
   /**
    * The Singleton Instance.
@@ -81,9 +81,6 @@
    */
   @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
   private final Vector<Command> m_additions = new Vector<>();
-  private NetworkTableEntry m_namesEntry;
-  private NetworkTableEntry m_idsEntry;
-  private NetworkTableEntry m_cancelEntry;
   /**
    * A list of all {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It is
    * created lazily.
@@ -98,11 +95,20 @@
   private Scheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
+    LiveWindow.setEnabledListener(() -> {
+      disable();
+      removeAll();
+    });
+    LiveWindow.setDisabledListener(() -> {
+      enable();
+    });
   }
 
   @Override
   public void close() {
     SendableRegistry.remove(this);
+    LiveWindow.setEnabledListener(null);
+    LiveWindow.setDisabledListener(null);
   }
 
   /**
@@ -319,13 +325,13 @@
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Scheduler");
-    m_namesEntry = builder.getEntry("Names");
-    m_idsEntry = builder.getEntry("Ids");
-    m_cancelEntry = builder.getEntry("Cancel");
+    final NetworkTableEntry namesEntry = builder.getEntry("Names");
+    final NetworkTableEntry idsEntry = builder.getEntry("Ids");
+    final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
     builder.setUpdateTable(() -> {
-      if (m_namesEntry != null && m_idsEntry != null && m_cancelEntry != null) {
+      if (namesEntry != null && idsEntry != null && cancelEntry != null) {
         // Get the commands to cancel
-        double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]);
+        double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
         if (toCancel.length > 0) {
           for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
             for (double d : toCancel) {
@@ -334,7 +340,7 @@
               }
             }
           }
-          m_cancelEntry.setDoubleArray(new double[0]);
+          cancelEntry.setDoubleArray(new double[0]);
         }
 
         if (m_runningCommandsChanged) {
@@ -351,8 +357,8 @@
             ids[number] = e.getData().hashCode();
             number++;
           }
-          m_namesEntry.setStringArray(commands);
-          m_idsEntry.setDoubleArray(ids);
+          namesEntry.setStringArray(commands);
+          idsEntry.setDoubleArray(ids);
         }
       }
     });

wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp

diff --git a/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp b/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
index f215083..0cce9d0 100644
--- a/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -66,6 +66,7 @@
 void Trigger::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Button");
   builder.SetSafeState([=]() { m_sendablePressed = false; });
-  builder.AddBooleanProperty("pressed", [=]() { return Grab(); },
-                             [=](bool value) { m_sendablePressed = value; });
+  builder.AddBooleanProperty(
+      "pressed", [=]() { return Grab(); },
+      [=](bool value) { m_sendablePressed = value; });
 }

wpilibOldCommands/src/main/native/cpp/commands/Command.cpp

diff --git a/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
index a7154c0..8c2abcf 100644
--- a/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -251,14 +251,15 @@
   builder.AddStringProperty(
       ".name", [=]() { return SendableRegistry::GetInstance().GetName(this); },
       nullptr);
-  builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
-                             [=](bool value) {
-                               if (value) {
-                                 if (!IsRunning()) Start();
-                               } else {
-                                 if (IsRunning()) Cancel();
-                               }
-                             });
-  builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
-                             nullptr);
+  builder.AddBooleanProperty(
+      "running", [=]() { return IsRunning(); },
+      [=](bool value) {
+        if (value) {
+          if (!IsRunning()) Start();
+        } else {
+          if (IsRunning()) Cancel();
+        }
+      });
+  builder.AddBooleanProperty(
+      ".isParented", [=]() { return IsParented(); }, nullptr);
 }

wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp

diff --git a/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
index f64ba67..31354e3 100644
--- a/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -137,7 +137,7 @@
       // If command timed out, cancel it so it's removed from the Scheduler
       if (entry->IsTimedOut()) cmd->_Cancel();
 
-      // If command finished or was cancelled, remove it from Scheduler
+      // If command finished or was canceled, remove it from Scheduler
       if (cmd->Run()) {
         break;
       } else {
@@ -192,7 +192,7 @@
       child->_Cancel();
     }
 
-    // If child finished or was cancelled, set it to nullptr. nullptr entries
+    // If child finished or was canceled, set it to nullptr. nullptr entries
     // are removed later.
     if (!child->Run()) {
       child->Removed();

wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp

diff --git a/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp b/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
index 1c75c65..3d89d92 100644
--- a/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -46,7 +46,7 @@
   }
 
   if (m_chosenCommand != nullptr) {
-    // This is a hack to make cancelling the chosen command inside a
+    // This is a hack to make canceling the chosen command inside a
     // CommandGroup work properly
     m_chosenCommand->ClearRequirements();
 

wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp

diff --git a/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
index 6944e41..47212e5 100644
--- a/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,6 +20,7 @@
 #include "frc/buttons/ButtonScheduler.h"
 #include "frc/commands/Command.h"
 #include "frc/commands/Subsystem.h"
+#include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -170,8 +171,8 @@
     // Cancel commands whose cancel buttons were pressed on the SmartDashboard
     if (!toCancel.empty()) {
       for (auto& command : m_impl->commands) {
-        for (const auto& cancelled : toCancel) {
-          if (command->GetID() == cancelled) {
+        for (const auto& canceled : toCancel) {
+          if (command->GetID() == canceled) {
             command->Cancel();
           }
         }
@@ -198,9 +199,20 @@
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand_Scheduler);
   SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = [this] {
+    this->SetEnabled(false);
+    this->RemoveAll();
+  };
+  scheduler->disabled = [this] { this->SetEnabled(true); };
 }
 
-Scheduler::~Scheduler() {}
+Scheduler::~Scheduler() {
+  SendableRegistry::GetInstance().Remove(this);
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = nullptr;
+  scheduler->disabled = nullptr;
+}
 
 void Scheduler::Impl::Remove(Command* command) {
   if (!commands.erase(command)) return;
@@ -220,7 +232,7 @@
   if (found == commands.end()) {
     // Check that the requirements can be had
     const auto& requirements = command->GetRequirements();
-    for (const auto& requirement : requirements) {
+    for (const auto requirement : requirements) {
       if (requirement->GetCurrentCommand() != nullptr &&
           !requirement->GetCurrentCommand()->IsInterruptible())
         return;

wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp

diff --git a/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp b/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
index 3178455..85e800e 100644
--- a/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
+++ b/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -122,11 +122,11 @@
 
   builder.AddBooleanProperty(
       ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
-  builder.AddStringProperty(".default",
-                            [=]() { return GetDefaultCommandName(); }, nullptr);
+  builder.AddStringProperty(
+      ".default", [=]() { return GetDefaultCommandName(); }, nullptr);
 
   builder.AddBooleanProperty(
       ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
-  builder.AddStringProperty(".command",
-                            [=]() { return GetCurrentCommandName(); }, nullptr);
+  builder.AddStringProperty(
+      ".command", [=]() { return GetCurrentCommandName(); }, nullptr);
 }

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

diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/Command.h b/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
index 241490a..cc6b795 100644
--- a/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -318,7 +318,7 @@
   /**
    * Clears list of subsystem requirements.
    *
-   * This is only used by ConditionalCommand so cancelling the chosen command
+   * This is only used by ConditionalCommand so canceling the chosen command
    * works properly in CommandGroup.
    */
   void ClearRequirements();
@@ -344,7 +344,7 @@
    * time-sensitive commands.
    *
    * Returning false will result in the command never ending automatically.
-   * It may still be cancelled manually or interrupted by another command.
+   * It may still be canceled manually or interrupted by another command.
    * Returning true will result in the command executing once and finishing
    * immediately. We recommend using InstantCommand for this.
    *

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

diff --git a/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h b/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
index 2afc05b..fd9b487 100644
--- a/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
+++ b/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -40,7 +40,7 @@
   PIDSubsystem(const wpi::Twine& name, double p, double i, double d);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * @param name the name
    * @param p    the proportional value
@@ -51,7 +51,7 @@
   PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * It will also space the time between PID loop calculations to be equal to
    * the given period.
@@ -78,7 +78,7 @@
   PIDSubsystem(double p, double i, double d);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * It will use the class name as its name.
    *
@@ -90,7 +90,7 @@
   PIDSubsystem(double p, double i, double d, double f);
 
   /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
+   * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
    *
    * It will use the class name as its name. It will also space the time
    * between PID loop calculations to be equal to the given period.

wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java

diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
index d97b352..fe7429b 100644
--- a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
 
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 
 public final class MockHardwareExtension implements BeforeAllCallback {
   private static ExtensionContext getRoot(ExtensionContext context) {
@@ -21,7 +21,7 @@
 
   @Override
   public void beforeAll(ExtensionContext context) {
-    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initalized", key -> {
+    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initialized", key -> {
       initializeHardware();
       return true;
     }, Boolean.class);

wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java

diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
index 5bc1949..108c49b 100644
--- a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,35 +21,36 @@
     final MockCommand command1 = new MockCommand();
     final MockCommand command2 = new MockCommand();
 
-    final CommandGroup commandGroup = new CommandGroup();
-    commandGroup.addParallel(command1);
-    commandGroup.addParallel(command2);
+    try (CommandGroup commandGroup = new CommandGroup()) {
+      commandGroup.addParallel(command1);
+      commandGroup.addParallel(command2);
 
-    assertCommandState(command1, 0, 0, 0, 0, 0);
-    assertCommandState(command2, 0, 0, 0, 0, 0);
-    commandGroup.start();
-    assertCommandState(command1, 0, 0, 0, 0, 0);
-    assertCommandState(command2, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 0, 0, 0, 0, 0);
-    assertCommandState(command2, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 0);
-    assertCommandState(command2, 1, 1, 1, 0, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 2, 2, 0, 0);
-    assertCommandState(command2, 1, 2, 2, 0, 0);
-    command1.setHasFinished(true);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 3, 3, 1, 0);
-    assertCommandState(command2, 1, 3, 3, 0, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 3, 3, 1, 0);
-    assertCommandState(command2, 1, 4, 4, 0, 0);
-    command2.setHasFinished(true);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 3, 3, 1, 0);
-    assertCommandState(command2, 1, 5, 5, 1, 0);
+      assertCommandState(command1, 0, 0, 0, 0, 0);
+      assertCommandState(command2, 0, 0, 0, 0, 0);
+      commandGroup.start();
+      assertCommandState(command1, 0, 0, 0, 0, 0);
+      assertCommandState(command2, 0, 0, 0, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 0, 0, 0, 0, 0);
+      assertCommandState(command2, 0, 0, 0, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 0);
+      assertCommandState(command2, 1, 1, 1, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 2, 2, 0, 0);
+      assertCommandState(command2, 1, 2, 2, 0, 0);
+      command1.setHasFinished(true);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 3, 3, 1, 0);
+      assertCommandState(command2, 1, 3, 3, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 3, 3, 1, 0);
+      assertCommandState(command2, 1, 4, 4, 0, 0);
+      command2.setHasFinished(true);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 3, 3, 1, 0);
+      assertCommandState(command2, 1, 5, 5, 1, 0);
+    }
   }
 
 }

wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java

diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
index 680a55d..cc2b76c 100644
--- a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,7 +23,7 @@
    */
   @Test
   public void testThreeCommandOnSubSystem() {
-    logger.fine("Begining Test");
+    logger.fine("Beginning Test");
     final ASubsystem subsystem = new ASubsystem();
 
     logger.finest("Creating Mock Command1");
@@ -34,60 +34,61 @@
     final MockCommand command3 = new MockCommand(subsystem);
 
     logger.finest("Creating Command Group");
-    final CommandGroup commandGroup = new CommandGroup();
-    commandGroup.addSequential(command1, 1.0);
-    commandGroup.addSequential(command2, 2.0);
-    commandGroup.addSequential(command3);
+    try (CommandGroup commandGroup = new CommandGroup()) {
+      commandGroup.addSequential(command1, 1.0);
+      commandGroup.addSequential(command2, 2.0);
+      commandGroup.addSequential(command3);
 
 
-    assertCommandState(command1, 0, 0, 0, 0, 0);
-    assertCommandState(command2, 0, 0, 0, 0, 0);
-    assertCommandState(command3, 0, 0, 0, 0, 0);
-    logger.finest("Starting Command group");
-    commandGroup.start();
-    assertCommandState(command1, 0, 0, 0, 0, 0);
-    assertCommandState(command2, 0, 0, 0, 0, 0);
-    assertCommandState(command3, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 0, 0, 0, 0, 0);
-    assertCommandState(command2, 0, 0, 0, 0, 0);
-    assertCommandState(command3, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 0);
-    assertCommandState(command2, 0, 0, 0, 0, 0);
-    assertCommandState(command3, 0, 0, 0, 0, 0);
-    sleep(1250); // command 1 timeout
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 1);
-    assertCommandState(command2, 1, 1, 1, 0, 0);
-    assertCommandState(command3, 0, 0, 0, 0, 0);
+      assertCommandState(command1, 0, 0, 0, 0, 0);
+      assertCommandState(command2, 0, 0, 0, 0, 0);
+      assertCommandState(command3, 0, 0, 0, 0, 0);
+      logger.finest("Starting Command group");
+      commandGroup.start();
+      assertCommandState(command1, 0, 0, 0, 0, 0);
+      assertCommandState(command2, 0, 0, 0, 0, 0);
+      assertCommandState(command3, 0, 0, 0, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 0, 0, 0, 0, 0);
+      assertCommandState(command2, 0, 0, 0, 0, 0);
+      assertCommandState(command3, 0, 0, 0, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 0);
+      assertCommandState(command2, 0, 0, 0, 0, 0);
+      assertCommandState(command3, 0, 0, 0, 0, 0);
+      sleep(1250); // command 1 timeout
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 1);
+      assertCommandState(command2, 1, 1, 1, 0, 0);
+      assertCommandState(command3, 0, 0, 0, 0, 0);
 
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 1);
-    assertCommandState(command2, 1, 2, 2, 0, 0);
-    assertCommandState(command3, 0, 0, 0, 0, 0);
-    sleep(2500); // command 2 timeout
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 1);
-    assertCommandState(command2, 1, 2, 2, 0, 1);
-    assertCommandState(command3, 1, 1, 1, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 1);
+      assertCommandState(command2, 1, 2, 2, 0, 0);
+      assertCommandState(command3, 0, 0, 0, 0, 0);
+      sleep(2500); // command 2 timeout
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 1);
+      assertCommandState(command2, 1, 2, 2, 0, 1);
+      assertCommandState(command3, 1, 1, 1, 0, 0);
 
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 1);
-    assertCommandState(command2, 1, 2, 2, 0, 1);
-    assertCommandState(command3, 1, 2, 2, 0, 0);
-    command3.setHasFinished(true);
-    assertCommandState(command1, 1, 1, 1, 0, 1);
-    assertCommandState(command2, 1, 2, 2, 0, 1);
-    assertCommandState(command3, 1, 2, 2, 0, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 1);
-    assertCommandState(command2, 1, 2, 2, 0, 1);
-    assertCommandState(command3, 1, 3, 3, 1, 0);
-    Scheduler.getInstance().run();
-    assertCommandState(command1, 1, 1, 1, 0, 1);
-    assertCommandState(command2, 1, 2, 2, 0, 1);
-    assertCommandState(command3, 1, 3, 3, 1, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 1);
+      assertCommandState(command2, 1, 2, 2, 0, 1);
+      assertCommandState(command3, 1, 2, 2, 0, 0);
+      command3.setHasFinished(true);
+      assertCommandState(command1, 1, 1, 1, 0, 1);
+      assertCommandState(command2, 1, 2, 2, 0, 1);
+      assertCommandState(command3, 1, 2, 2, 0, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 1);
+      assertCommandState(command2, 1, 2, 2, 0, 1);
+      assertCommandState(command3, 1, 3, 3, 1, 0);
+      Scheduler.getInstance().run();
+      assertCommandState(command1, 1, 1, 1, 0, 1);
+      assertCommandState(command2, 1, 2, 2, 0, 1);
+      assertCommandState(command3, 1, 3, 3, 1, 0);
+    }
   }
 
 }

wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java

diff --git a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
index 16f088f..8e304eb 100644
--- a/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
+++ b/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,7 +25,6 @@
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 
-@SuppressWarnings({"PMD.TooManyMethods"})
 public class ShuffleboardTabTest {
   private NetworkTableInstance m_ntInstance;
   private ShuffleboardTab m_tab;

wpilibc/CMakeLists.txt

diff --git a/wpilibc/CMakeLists.txt b/wpilibc/CMakeLists.txt
index 977200a..6435040 100644
--- a/wpilibc/CMakeLists.txt
+++ b/wpilibc/CMakeLists.txt
@@ -17,14 +17,14 @@
                 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
                             $<INSTALL_INTERFACE:${include_dest}/wpilibc>)
 wpilib_target_warnings(wpilibc)
-target_link_libraries(wpilibc PUBLIC cameraserver hal ntcore cscore wpiutil ${OpenCV_LIBS})
+target_link_libraries(wpilibc PUBLIC cameraserver hal ntcore cscore wpimath wpiutil ${OpenCV_LIBS})
 
 set_property(TARGET wpilibc PROPERTY FOLDER "libraries")
 
 install(TARGETS wpilibc EXPORT wpilibc DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibc")
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
     set (wpilibc_config_dir ${wpilib_dest})
 else()
     set (wpilibc_config_dir share/wpilibc)

wpilibc/build.gradle

diff --git a/wpilibc/build.gradle b/wpilibc/build.gradle
index 4ee5766..ec85691 100644
--- a/wpilibc/build.gradle
+++ b/wpilibc/build.gradle
@@ -98,6 +98,7 @@
                 lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                 project(':hal').addHalDependency(it, 'shared')
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
             }
         }
         "${nativeName}"(NativeLibrarySpec) {
@@ -116,6 +117,7 @@
                 lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                 project(':hal').addHalDependency(it, 'shared')
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
 
                 if (it instanceof SharedLibraryBinarySpec) {
                     cppCompiler.define 'DYNAMIC_CAMERA_SERVER'
@@ -152,6 +154,7 @@
                 lib project: ':cscore', library: 'cscore', linkage: 'shared'
                 project(':hal').addHalDependency(it, 'shared')
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 nativeUtils.useRequiredLibrary(it, 'opencv_shared')
                 if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
@@ -201,6 +204,7 @@
             lib project: ':cscore', library: 'cscore', linkage: 'shared'
             project(':hal').addHalDependency(it, 'shared')
             lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
             lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
             nativeUtils.useRequiredLibrary(it, 'opencv_shared')
             lib library: nativeName, linkage: 'shared'

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

diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
index a760daa..aff0802 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,7 +21,7 @@
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
   // Turn on the measurements

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

diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
index 077a7ff..329b148 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,7 +20,7 @@
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
   m_spi.SetClockRate(500000);

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

diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp
index 6ed8c8c..aff572f 100644
--- a/wpilibc/src/main/native/cpp/ADXL362.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -40,7 +40,7 @@
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
 

wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp

diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
index 2cf2f73..8ea5b41 100644
--- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,7 +32,7 @@
 ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
 
 ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
-    : m_spi(port), m_simDevice("ADXRS450_Gyro", port) {
+    : m_spi(port), m_port(port), m_simDevice("ADXRS450_Gyro", port) {
   if (m_simDevice) {
     m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
     m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
@@ -123,3 +123,5 @@
   m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage());
   m_spi.ResetAccumulator();
 }
+
+int ADXRS450_Gyro::GetPort() const { return m_port; }

wpilibc/src/main/native/cpp/AddressableLED.cpp

diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp
index b0c3a45..81adc7b 100644
--- a/wpilibc/src/main/native/cpp/AddressableLED.cpp
+++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -94,36 +94,5 @@
 }
 
 void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
-  if (s == 0) {
-    SetRGB(v, v, v);
-    return;
-  }
-
-  int region = h / 30;
-  int remainder = (h - (region * 30)) * 6;
-
-  int p = (v * (255 - s)) >> 8;
-  int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
-  int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
-
-  switch (region) {
-    case 0:
-      SetRGB(v, t, p);
-      break;
-    case 1:
-      SetRGB(q, v, p);
-      break;
-    case 2:
-      SetRGB(p, v, t);
-      break;
-    case 3:
-      SetRGB(p, q, v);
-      break;
-    case 4:
-      SetRGB(t, p, v);
-      break;
-    default:
-      SetRGB(v, p, q);
-      break;
-  }
+  SetLED(Color::FromHSV(h, s, v));
 }

wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp

diff --git a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
index be0c7d2..09bc2f5 100644
--- a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,6 +9,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -52,8 +53,8 @@
 
 void AnalogAccelerometer::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Accelerometer");
-  builder.AddDoubleProperty("Value", [=]() { return GetAcceleration(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetAcceleration(); }, nullptr);
 }
 
 void AnalogAccelerometer::InitAccelerometer() {

wpilibc/src/main/native/cpp/AnalogEncoder.cpp

diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
index a194961..c1f4e8d 100644
--- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,7 @@
 #include "frc/AnalogEncoder.h"
 
 #include "frc/AnalogInput.h"
+#include "frc/Base.h"
 #include "frc/Counter.h"
 #include "frc/DriverStation.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -94,11 +95,13 @@
   m_positionOffset = m_analogInput->GetVoltage();
 }
 
+int AnalogEncoder::GetChannel() const { return m_analogInput->GetChannel(); }
+
 void AnalogEncoder::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("AbsoluteEncoder");
-  builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
-                            nullptr);
-  builder.AddDoubleProperty("Distance Per Rotation",
-                            [this] { return this->GetDistancePerRotation(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Distance", [this] { return this->GetDistance(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance Per Rotation",
+      [this] { return this->GetDistancePerRotation(); }, nullptr);
 }

wpilibc/src/main/native/cpp/AnalogGyro.cpp

diff --git a/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
index c891506..2ec4439 100644
--- a/wpilibc/src/main/native/cpp/AnalogGyro.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,6 +15,7 @@
 #include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogInput.h"
+#include "frc/Base.h"
 #include "frc/Timer.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -160,3 +161,7 @@
   HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
   wpi_setHALError(status);
 }
+
+std::shared_ptr<AnalogInput> AnalogGyro::GetAnalogInput() const {
+  return m_analog;
+}

wpilibc/src/main/native/cpp/AnalogInput.cpp

diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp
index c8197b7..11c30ee 100644
--- a/wpilibc/src/main/native/cpp/AnalogInput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,6 @@
 
 #include "frc/AnalogInput.h"
 
-#include <utility>
-
 #include <hal/AnalogAccumulator.h>
 #include <hal/AnalogInput.h>
 #include <hal/FRCUsageReporting.h>
@@ -230,6 +228,6 @@
 
 void AnalogInput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Analog Input");
-  builder.AddDoubleProperty("Value", [=]() { return GetAverageVoltage(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetAverageVoltage(); }, nullptr);
 }

wpilibc/src/main/native/cpp/AnalogOutput.cpp

diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
index 041b446..bd8126a 100644
--- a/wpilibc/src/main/native/cpp/AnalogOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -65,10 +65,11 @@
   return voltage;
 }
 
-int AnalogOutput::GetChannel() { return m_channel; }
+int AnalogOutput::GetChannel() const { return m_channel; }
 
 void AnalogOutput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Analog Output");
-  builder.AddDoubleProperty("Value", [=]() { return GetVoltage(); },
-                            [=](double value) { SetVoltage(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetVoltage(); },
+      [=](double value) { SetVoltage(value); });
 }

wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp

diff --git a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
index b650ec7..c7aedd6 100644
--- a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,7 @@
 
 #include "frc/AnalogPotentiometer.h"
 
+#include "frc/Base.h"
 #include "frc/RobotController.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -34,7 +35,8 @@
 }
 
 double AnalogPotentiometer::Get() const {
-  return (m_analog_input->GetVoltage() / RobotController::GetVoltage5V()) *
+  return (m_analog_input->GetAverageVoltage() /
+          RobotController::GetVoltage5V()) *
              m_fullRange +
          m_offset;
 }
@@ -42,5 +44,7 @@
 double AnalogPotentiometer::PIDGet() { return Get(); }
 
 void AnalogPotentiometer::InitSendable(SendableBuilder& builder) {
-  m_analog_input->InitSendable(builder);
+  builder.SetSmartDashboardType("Analog Input");
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }

wpilibc/src/main/native/cpp/AnalogTrigger.cpp

diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
index ddbb7c8..45821ba 100644
--- a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 #include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogInput.h"
+#include "frc/Base.h"
 #include "frc/DutyCycle.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableRegistry.h"

wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp

diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
index d7cdef6..efc0fe6 100644
--- a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -43,7 +43,10 @@
 
 void BuiltInAccelerometer::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("3AxisAccelerometer");
-  builder.AddDoubleProperty("X", [=]() { return GetX(); }, nullptr);
-  builder.AddDoubleProperty("Y", [=]() { return GetY(); }, nullptr);
-  builder.AddDoubleProperty("Z", [=]() { return GetZ(); }, nullptr);
+  builder.AddDoubleProperty(
+      "X", [=]() { return GetX(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Y", [=]() { return GetY(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Z", [=]() { return GetZ(); }, nullptr);
 }

wpilibc/src/main/native/cpp/CAN.cpp

diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp
index 797b9ff..e56e435 100644
--- a/wpilibc/src/main/native/cpp/CAN.cpp
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -70,6 +70,25 @@
   wpi_setHALError(status);
 }
 
+int CAN::WritePacketNoError(const uint8_t* data, int length, int apiId) {
+  int32_t status = 0;
+  HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
+  return status;
+}
+
+int CAN::WritePacketRepeatingNoError(const uint8_t* data, int length, int apiId,
+                                     int repeatMs) {
+  int32_t status = 0;
+  HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
+  return status;
+}
+
+int CAN::WriteRTRFrameNoError(int length, int apiId) {
+  int32_t status = 0;
+  HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
+  return status;
+}
+
 void CAN::StopPacketRepeating(int apiId) {
   int32_t status = 0;
   HAL_StopCANPacketRepeating(m_handle, apiId, &status);

wpilibc/src/main/native/cpp/Compressor.cpp

diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 29789be..e40758f 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -204,15 +204,18 @@
   }
 }
 
+int Compressor::GetModule() const { return m_module; }
+
 void Compressor::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Compressor");
-  builder.AddBooleanProperty("Enabled", [=]() { return Enabled(); },
-                             [=](bool value) {
-                               if (value)
-                                 Start();
-                               else
-                                 Stop();
-                             });
+  builder.AddBooleanProperty(
+      "Enabled", [=]() { return Enabled(); },
+      [=](bool value) {
+        if (value)
+          Start();
+        else
+          Stop();
+      });
   builder.AddBooleanProperty(
       "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
 }

wpilibc/src/main/native/cpp/Counter.cpp

diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
index d2158ca..ca74ab8 100644
--- a/wpilibc/src/main/native/cpp/Counter.cpp
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,6 +13,7 @@
 #include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogTrigger.h"
+#include "frc/Base.h"
 #include "frc/DigitalInput.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -332,5 +333,6 @@
 
 void Counter::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Counter");
-  builder.AddDoubleProperty("Value", [=]() { return Get(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }

wpilibc/src/main/native/cpp/DigitalInput.cpp

diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index 7210750..d82f570 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/DigitalInput.h"
 
 #include <limits>
-#include <utility>
 
 #include <hal/DIO.h>
 #include <hal/FRCUsageReporting.h>
@@ -73,5 +72,6 @@
 
 void DigitalInput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Digital Input");
-  builder.AddBooleanProperty("Value", [=]() { return Get(); }, nullptr);
+  builder.AddBooleanProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }

wpilibc/src/main/native/cpp/DigitalOutput.cpp

diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
index 6e9b09b..0bdc57c 100644
--- a/wpilibc/src/main/native/cpp/DigitalOutput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/DigitalOutput.h"
 
 #include <limits>
-#include <utility>
 
 #include <hal/DIO.h>
 #include <hal/FRCUsageReporting.h>
@@ -154,6 +153,6 @@
 
 void DigitalOutput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Digital Output");
-  builder.AddBooleanProperty("Value", [=]() { return Get(); },
-                             [=](bool value) { Set(value); });
+  builder.AddBooleanProperty(
+      "Value", [=]() { return Get(); }, [=](bool value) { Set(value); });
 }

wpilibc/src/main/native/cpp/DoubleSolenoid.cpp

diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
index c896ea8..67eb0b7 100644
--- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -92,6 +92,7 @@
 
   bool forward = false;
   bool reverse = false;
+
   switch (value) {
     case kOff:
       forward = false;
@@ -106,6 +107,7 @@
       reverse = true;
       break;
   }
+
   int fstatus = 0;
   HAL_SetSolenoid(m_forwardHandle, forward, &fstatus);
   int rstatus = 0;
@@ -117,6 +119,7 @@
 
 DoubleSolenoid::Value DoubleSolenoid::Get() const {
   if (StatusIsFatal()) return kOff;
+
   int fstatus = 0;
   int rstatus = 0;
   bool valueForward = HAL_GetSolenoid(m_forwardHandle, &fstatus);
@@ -125,9 +128,23 @@
   wpi_setHALError(fstatus);
   wpi_setHALError(rstatus);
 
-  if (valueForward) return kForward;
-  if (valueReverse) return kReverse;
-  return kOff;
+  if (valueForward) {
+    return kForward;
+  } else if (valueReverse) {
+    return kReverse;
+  } else {
+    return kOff;
+  }
+}
+
+void DoubleSolenoid::Toggle() {
+  Value value = Get();
+
+  if (value == kForward) {
+    Set(kReverse);
+  } else if (value == kReverse) {
+    Set(kForward);
+  }
 }
 
 bool DoubleSolenoid::IsFwdSolenoidBlackListed() const {

wpilibc/src/main/native/cpp/DriverStation.cpp

diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
index 7c64883..dbb99f9 100644
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 #include <chrono>
 
 #include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Power.h>
 #include <networktables/NetworkTable.h>
@@ -19,10 +18,8 @@
 #include <wpi/SmallString.h>
 #include <wpi/StringRef.h>
 
-#include "frc/AnalogInput.h"
 #include "frc/MotorSafety.h"
 #include "frc/Timer.h"
-#include "frc/Utility.h"
 #include "frc/WPIErrors.h"
 
 namespace frc {
@@ -68,6 +65,15 @@
 
 static constexpr double kJoystickUnpluggedMessageInterval = 1.0;
 
+static int& GetDSLastCount() {
+  // There is a rollover error condition here. At Packet# = n * (uintmax), this
+  // will return false when instead it should return true. However, this at a
+  // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
+  // worth the cycles to check.
+  thread_local int lastCount{0};
+  return lastCount;
+}
+
 DriverStation::~DriverStation() {
   m_isRunning = false;
   // Trigger a DS mutex release in case there is no driver station running.
@@ -325,6 +331,11 @@
   return static_cast<bool>(descriptor.axisTypes);
 }
 
+bool DriverStation::IsJoystickConnected(int stick) const {
+  return GetStickAxisCount(stick) > 0 || GetStickButtonCount(stick) > 0 ||
+         GetStickPOVCount(stick) > 0;
+}
+
 bool DriverStation::IsEnabled() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
@@ -349,12 +360,24 @@
   return controlWord.autonomous;
 }
 
+bool DriverStation::IsAutonomousEnabled() const {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.autonomous && controlWord.enabled;
+}
+
 bool DriverStation::IsOperatorControl() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return !(controlWord.autonomous || controlWord.test);
 }
 
+bool DriverStation::IsOperatorControlEnabled() const {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return !controlWord.autonomous && !controlWord.test && controlWord.enabled;
+}
+
 bool DriverStation::IsTest() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
@@ -367,7 +390,14 @@
   return controlWord.dsAttached;
 }
 
-bool DriverStation::IsNewControlData() const { return HAL_IsNewControlData(); }
+bool DriverStation::IsNewControlData() const {
+  std::unique_lock lock(m_waitForDataMutex);
+  int& lastCount = GetDSLastCount();
+  int currentCount = m_waitForDataCounter;
+  if (lastCount == currentCount) return false;
+  lastCount = currentCount;
+  return true;
+}
 
 bool DriverStation::IsFMSAttached() const {
   HAL_ControlWord controlWord;
@@ -448,7 +478,12 @@
       std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
 
   std::unique_lock lock(m_waitForDataMutex);
+  int& lastCount = GetDSLastCount();
   int currentCount = m_waitForDataCounter;
+  if (lastCount != currentCount) {
+    lastCount = currentCount;
+    return true;
+  }
   while (m_waitForDataCounter == currentCount) {
     if (timeout > 0) {
       auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
@@ -459,6 +494,7 @@
       m_waitForDataCond.wait(lock);
     }
   }
+  lastCount = m_waitForDataCounter;
   return true;
 }
 
@@ -507,6 +543,14 @@
   SendMatchData();
 }
 
+void DriverStation::SilenceJoystickConnectionWarning(bool silence) {
+  m_silenceJoystickWarning = silence;
+}
+
+bool DriverStation::IsJoystickConnectionWarningSilenced() const {
+  return !IsFMSAttached() && m_silenceJoystickWarning;
+}
+
 DriverStation::DriverStation() {
   HAL_Initialize(500, 0);
   m_waitForDataCounter = 0;
@@ -534,10 +578,12 @@
 }
 
 void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
-  double currentTime = Timer::GetFPGATimestamp();
-  if (currentTime > m_nextMessageTime) {
-    ReportWarning(message);
-    m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+  if (IsFMSAttached() || !m_silenceJoystickWarning) {
+    double currentTime = Timer::GetFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      ReportWarning(message);
+      m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+    }
   }
 }
 

wpilibc/src/main/native/cpp/DutyCycle.cpp

diff --git a/wpilibc/src/main/native/cpp/DutyCycle.cpp b/wpilibc/src/main/native/cpp/DutyCycle.cpp
index 12f390e..7e4e98d 100644
--- a/wpilibc/src/main/native/cpp/DutyCycle.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycle.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,7 @@
 #include <hal/DutyCycle.h>
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/DigitalSource.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -93,8 +94,8 @@
 
 void DutyCycle::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Duty Cycle");
-  builder.AddDoubleProperty("Frequency",
-                            [this] { return this->GetFrequency(); }, nullptr);
-  builder.AddDoubleProperty("Output", [this] { return this->GetOutput(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Frequency", [this] { return this->GetFrequency(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Output", [this] { return this->GetOutput(); }, nullptr);
 }

wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp

diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
index ea054ce..4fc5457 100644
--- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,7 @@
 
 #include "frc/DutyCycleEncoder.h"
 
+#include "frc/Base.h"
 #include "frc/Counter.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalSource.h"
@@ -18,51 +19,37 @@
 
 DutyCycleEncoder::DutyCycleEncoder(int channel)
     : m_dutyCycle{std::make_shared<DutyCycle>(
-          std::make_shared<DigitalInput>(channel))},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+          std::make_shared<DigitalInput>(channel))} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
-    : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
-    : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
-    : m_dutyCycle{std::move(dutyCycle)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::move(dutyCycle)} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource)
-    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource)
-    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource)
-    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
   Init();
 }
 
@@ -71,15 +58,19 @@
 
   if (m_simDevice) {
     m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+    m_simDistancePerRotation =
+        m_simDevice.CreateDouble("DistancePerRotation", false, 1.0);
     m_simIsConnected = m_simDevice.CreateBoolean("Connected", false, true);
+  } else {
+    m_analogTrigger = std::make_unique<AnalogTrigger>(m_dutyCycle.get());
+    m_analogTrigger->SetLimitsDutyCycle(0.25, 0.75);
+    m_counter = std::make_unique<Counter>();
+    m_counter->SetUpSource(
+        m_analogTrigger->CreateOutput(AnalogTriggerType::kRisingPulse));
+    m_counter->SetDownSource(
+        m_analogTrigger->CreateOutput(AnalogTriggerType::kFallingPulse));
   }
 
-  m_analogTrigger.SetLimitsDutyCycle(0.25, 0.75);
-  m_counter.SetUpSource(
-      m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
-  m_counter.SetDownSource(
-      m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
-
   SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
                                         m_dutyCycle->GetSourceChannel());
 }
@@ -90,9 +81,9 @@
   // As the values are not atomic, keep trying until we get 2 reads of the same
   // value If we don't within 10 attempts, error
   for (int i = 0; i < 10; i++) {
-    auto counter = m_counter.Get();
+    auto counter = m_counter->Get();
     auto pos = m_dutyCycle->GetOutput();
-    auto counter2 = m_counter.Get();
+    auto counter2 = m_counter->Get();
     auto pos2 = m_dutyCycle->GetOutput();
     if (counter == counter2 && pos == pos2) {
       units::turn_t turns{counter + pos - m_positionOffset};
@@ -109,6 +100,7 @@
 
 void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
   m_distancePerRotation = distancePerRotation;
+  m_simDistancePerRotation.Set(distancePerRotation);
 }
 
 double DutyCycleEncoder::GetDistancePerRotation() const {
@@ -124,7 +116,7 @@
 }
 
 void DutyCycleEncoder::Reset() {
-  m_counter.Reset();
+  if (m_counter) m_counter->Reset();
   m_positionOffset = m_dutyCycle->GetOutput();
 }
 
@@ -140,13 +132,21 @@
   m_frequencyThreshold = frequency;
 }
 
+int DutyCycleEncoder::GetFPGAIndex() const {
+  return m_dutyCycle->GetFPGAIndex();
+}
+
+int DutyCycleEncoder::GetSourceChannel() const {
+  return m_dutyCycle->GetSourceChannel();
+}
+
 void DutyCycleEncoder::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("AbsoluteEncoder");
-  builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
-                            nullptr);
-  builder.AddDoubleProperty("Distance Per Rotation",
-                            [this] { return this->GetDistancePerRotation(); },
-                            nullptr);
-  builder.AddDoubleProperty("Is Connected",
-                            [this] { return this->IsConnected(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance", [this] { return this->GetDistance(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance Per Rotation",
+      [this] { return this->GetDistancePerRotation(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Is Connected", [this] { return this->IsConnected(); }, nullptr);
 }

wpilibc/src/main/native/cpp/Encoder.cpp

diff --git a/wpilibc/src/main/native/cpp/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp
index 963bcc6..87bb45f 100644
--- a/wpilibc/src/main/native/cpp/Encoder.cpp
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 #include <hal/Encoder.h>
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/DigitalInput.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -236,11 +237,12 @@
   else
     builder.SetSmartDashboardType("Encoder");
 
-  builder.AddDoubleProperty("Speed", [=]() { return GetRate(); }, nullptr);
-  builder.AddDoubleProperty("Distance", [=]() { return GetDistance(); },
-                            nullptr);
-  builder.AddDoubleProperty("Distance per Tick",
-                            [=]() { return GetDistancePerPulse(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Speed", [=]() { return GetRate(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance", [=]() { return GetDistance(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance per Tick", [=]() { return GetDistancePerPulse(); }, nullptr);
 }
 
 void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {

wpilibc/src/main/native/cpp/Error.cpp

diff --git a/wpilibc/src/main/native/cpp/Error.cpp b/wpilibc/src/main/native/cpp/Error.cpp
index ee7dcbd..5e072c9 100644
--- a/wpilibc/src/main/native/cpp/Error.cpp
+++ b/wpilibc/src/main/native/cpp/Error.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,9 +10,9 @@
 #include <wpi/Path.h>
 #include <wpi/StackTrace.h>
 
+#include "frc/Base.h"
 #include "frc/DriverStation.h"
 #include "frc/Timer.h"
-#include "frc/Utility.h"
 
 using namespace frc;
 

wpilibc/src/main/native/cpp/ErrorBase.cpp

diff --git a/wpilibc/src/main/native/cpp/ErrorBase.cpp b/wpilibc/src/main/native/cpp/ErrorBase.cpp
index d098c07..8c7c5a2 100644
--- a/wpilibc/src/main/native/cpp/ErrorBase.cpp
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,17 +8,17 @@
 #include "frc/ErrorBase.h"
 
 #include <cerrno>
-#include <cstdio>
 #include <cstring>
 #include <set>
+#include <utility>
 
-#include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <wpi/Format.h>
 #include <wpi/SmallString.h>
+#include <wpi/mutex.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/WPIErrors.h"
+#include "frc/Base.h"
 
 using namespace frc;
 

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

diff --git a/wpilibc/src/main/native/cpp/Filesystem.cpp b/wpilibc/src/main/native/cpp/Filesystem.cpp
index 1546050..7d2ea1d 100644
--- a/wpilibc/src/main/native/cpp/Filesystem.cpp
+++ b/wpilibc/src/main/native/cpp/Filesystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,7 +18,7 @@
 
 void frc::filesystem::GetOperatingDirectory(
     wpi::SmallVectorImpl<char>& result) {
-  if (RobotBase::IsReal()) {
+  if constexpr (RobotBase::IsReal()) {
     wpi::sys::path::native("/home/lvuser", result);
   } else {
     frc::filesystem::GetLaunchDirectory(result);
@@ -27,5 +27,11 @@
 
 void frc::filesystem::GetDeployDirectory(wpi::SmallVectorImpl<char>& result) {
   frc::filesystem::GetOperatingDirectory(result);
-  wpi::sys::path::append(result, "deploy");
+  if constexpr (RobotBase::IsReal()) {
+    wpi::sys::path::append(result, "deploy");
+  } else {
+    wpi::sys::path::append(result, "src");
+    wpi::sys::path::append(result, "main");
+    wpi::sys::path::append(result, "deploy");
+  }
 }

wpilibc/src/main/native/cpp/GenericHID.cpp

diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp
index a7dcfc0..4a264c8 100644
--- a/wpilibc/src/main/native/cpp/GenericHID.cpp
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/GenericHID.h"
 
 #include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 #include "frc/WPIErrors.h"
@@ -48,6 +47,10 @@
   return m_ds->GetStickButtonCount(m_port);
 }
 
+bool GenericHID::IsConnected() const {
+  return m_ds->IsJoystickConnected(m_port);
+}
+
 GenericHID::HIDType GenericHID::GetType() const {
   return static_cast<HIDType>(m_ds->GetJoystickType(m_port));
 }

wpilibc/src/main/native/cpp/GyroBase.cpp

diff --git a/wpilibc/src/main/native/cpp/GyroBase.cpp b/wpilibc/src/main/native/cpp/GyroBase.cpp
index ec67675..ba9b2f0 100644
--- a/wpilibc/src/main/native/cpp/GyroBase.cpp
+++ b/wpilibc/src/main/native/cpp/GyroBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,5 +25,6 @@
 
 void GyroBase::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Gyro");
-  builder.AddDoubleProperty("Value", [=]() { return GetAngle(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetAngle(); }, nullptr);
 }

wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp

diff --git a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
index 202c61d..744e349 100644
--- a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,6 @@
 
 #include "frc/InterruptableSensorBase.h"
 
-#include <hal/FRCUsageReporting.h>
-
 #include "frc/Utility.h"
 #include "frc/WPIErrors.h"
 

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

diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
index c8664a5..72fb79b 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,6 +24,10 @@
 void IterativeRobot::StartCompetition() {
   RobotInit();
 
+  if constexpr (IsSimulation()) {
+    SimulationInit();
+  }
+
   // Tell the DS that the robot is ready to be enabled
   HAL_ObserveUserProgramStarting();
 

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index 2997234..7b29130 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,16 +7,12 @@
 
 #include "frc/IterativeRobotBase.h"
 
-#include <cstdio>
-
 #include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
 #include <wpi/Format.h>
 #include <wpi/SmallString.h>
 #include <wpi/raw_ostream.h>
 
 #include "frc/DriverStation.h"
-#include "frc/Timer.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/shuffleboard/Shuffleboard.h"
 #include "frc/smartdashboard/SmartDashboard.h"
@@ -34,6 +30,10 @@
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
 }
 
+void IterativeRobotBase::SimulationInit() {
+  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
 void IterativeRobotBase::DisabledInit() {
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
 }
@@ -58,6 +58,14 @@
   }
 }
 
+void IterativeRobotBase::SimulationPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    firstRun = false;
+  }
+}
+
 void IterativeRobotBase::DisabledPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
@@ -161,6 +169,12 @@
   m_watchdog.AddEpoch("LiveWindow::UpdateValues()");
   Shuffleboard::Update();
   m_watchdog.AddEpoch("Shuffleboard::Update()");
+
+  if constexpr (IsSimulation()) {
+    SimulationPeriodic();
+    m_watchdog.AddEpoch("SimulationPeriodic()");
+  }
+
   m_watchdog.Disable();
 
   // Warn on loop time overruns

wpilibc/src/main/native/cpp/Joystick.cpp

diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp
index 8d464cf..c2cdaea 100644
--- a/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,9 +12,6 @@
 #include <hal/FRCUsageReporting.h>
 #include <wpi/math>
 
-#include "frc/DriverStation.h"
-#include "frc/WPIErrors.h"
-
 using namespace frc;
 
 Joystick::Joystick(int port) : GenericHID(port) {

wpilibc/src/main/native/cpp/NidecBrushless.cpp

diff --git a/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
index 5bce36e..82a278e 100644
--- a/wpilibc/src/main/native/cpp/NidecBrushless.cpp
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -70,6 +70,6 @@
   builder.SetSmartDashboardType("Nidec Brushless");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { StopMotor(); });
-  builder.AddDoubleProperty("Value", [=]() { return Get(); },
-                            [=](double value) { Set(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
 }

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

diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp
index a7fa038..4280ce4 100644
--- a/wpilibc/src/main/native/cpp/Notifier.cpp
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,7 @@
 
 #include <hal/FRCUsageReporting.h>
 #include <hal/Notifier.h>
+#include <hal/Threads.h>
 #include <wpi/SmallString.h>
 
 #include "frc/Timer.h"
@@ -54,6 +55,42 @@
   });
 }
 
+Notifier::Notifier(int priority, std::function<void()> handler) {
+  if (handler == nullptr)
+    wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+  m_handler = handler;
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  wpi_setHALError(status);
+
+  m_thread = std::thread([=] {
+    int32_t status = 0;
+    HAL_SetCurrentThreadPriority(true, priority, &status);
+    for (;;) {
+      HAL_NotifierHandle notifier = m_notifier.load();
+      if (notifier == 0) break;
+      uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+      if (curTime == 0 || status != 0) break;
+
+      std::function<void()> handler;
+      {
+        std::scoped_lock lock(m_processMutex);
+        handler = m_handler;
+        if (m_periodic) {
+          m_expirationTime += m_period;
+          UpdateAlarm();
+        } else {
+          // need to update the alarm to cause it to wait again
+          UpdateAlarm(UINT64_MAX);
+        }
+      }
+
+      // call callback
+      if (handler) handler();
+    }
+  });
+}
+
 Notifier::~Notifier() {
   int32_t status = 0;
   // atomically set handle to 0, then clean
@@ -129,6 +166,8 @@
 }
 
 void Notifier::Stop() {
+  std::scoped_lock lock(m_processMutex);
+  m_periodic = false;
   int32_t status = 0;
   HAL_CancelNotifierAlarm(m_notifier, &status);
   wpi_setHALError(status);

wpilibc/src/main/native/cpp/PIDBase.cpp

diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp
index c8f6fed..5802d98 100644
--- a/wpilibc/src/main/native/cpp/PIDBase.cpp
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -229,16 +229,17 @@
 void PIDBase::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("PIDController");
   builder.SetSafeState([=]() { Reset(); });
-  builder.AddDoubleProperty("p", [=]() { return GetP(); },
-                            [=](double value) { SetP(value); });
-  builder.AddDoubleProperty("i", [=]() { return GetI(); },
-                            [=](double value) { SetI(value); });
-  builder.AddDoubleProperty("d", [=]() { return GetD(); },
-                            [=](double value) { SetD(value); });
-  builder.AddDoubleProperty("f", [=]() { return GetF(); },
-                            [=](double value) { SetF(value); });
-  builder.AddDoubleProperty("setpoint", [=]() { return GetSetpoint(); },
-                            [=](double value) { SetSetpoint(value); });
+  builder.AddDoubleProperty(
+      "p", [=]() { return GetP(); }, [=](double value) { SetP(value); });
+  builder.AddDoubleProperty(
+      "i", [=]() { return GetI(); }, [=](double value) { SetI(value); });
+  builder.AddDoubleProperty(
+      "d", [=]() { return GetD(); }, [=](double value) { SetD(value); });
+  builder.AddDoubleProperty(
+      "f", [=]() { return GetF(); }, [=](double value) { SetF(value); });
+  builder.AddDoubleProperty(
+      "setpoint", [=]() { return GetSetpoint(); },
+      [=](double value) { SetSetpoint(value); });
 }
 
 void PIDBase::Calculate() {

wpilibc/src/main/native/cpp/PIDController.cpp

diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp
index 0c86f55..a90a645 100644
--- a/wpilibc/src/main/native/cpp/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -80,6 +80,7 @@
 
 void PIDController::InitSendable(SendableBuilder& builder) {
   PIDBase::InitSendable(builder);
-  builder.AddBooleanProperty("enabled", [=]() { return IsEnabled(); },
-                             [=](bool value) { SetEnabled(value); });
+  builder.AddBooleanProperty(
+      "enabled", [=]() { return IsEnabled(); },
+      [=](bool value) { SetEnabled(value); });
 }

wpilibc/src/main/native/cpp/PWM.cpp

diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp
index dabcd2e..c2cd2e2 100644
--- a/wpilibc/src/main/native/cpp/PWM.cpp
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -199,6 +199,7 @@
   builder.SetSmartDashboardType("PWM");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { SetDisabled(); });
-  builder.AddDoubleProperty("Value", [=]() { return GetRaw(); },
-                            [=](double value) { SetRaw(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetRaw(); },
+      [=](double value) { SetRaw(value); });
 }

wpilibc/src/main/native/cpp/PWMSpeedController.cpp

diff --git a/wpilibc/src/main/native/cpp/PWMSpeedController.cpp b/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
index ea298de..ec0be7b 100644
--- a/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
+++ b/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -35,6 +35,7 @@
   builder.SetSmartDashboardType("Speed Controller");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { SetDisabled(); });
-  builder.AddDoubleProperty("Value", [=]() { return GetSpeed(); },
-                            [=](double value) { SetSpeed(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetSpeed(); },
+      [=](double value) { SetSpeed(value); });
 }

wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp

diff --git a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
index 7d7d916..4070633 100644
--- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,7 +25,7 @@
 /**
  * Initialize the PDP.
  */
-PowerDistributionPanel::PowerDistributionPanel(int module) {
+PowerDistributionPanel::PowerDistributionPanel(int module) : m_module(module) {
   int32_t status = 0;
   m_handle = HAL_InitializePDP(module, &status);
   if (status != 0) {
@@ -136,13 +136,16 @@
   }
 }
 
+int PowerDistributionPanel::GetModule() const { return m_module; }
+
 void PowerDistributionPanel::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("PowerDistributionPanel");
   for (int i = 0; i < SensorUtil::kPDPChannels; ++i) {
-    builder.AddDoubleProperty("Chan" + wpi::Twine(i),
-                              [=]() { return GetCurrent(i); }, nullptr);
+    builder.AddDoubleProperty(
+        "Chan" + wpi::Twine(i), [=]() { return GetCurrent(i); }, nullptr);
   }
-  builder.AddDoubleProperty("Voltage", [=]() { return GetVoltage(); }, nullptr);
-  builder.AddDoubleProperty("TotalCurrent", [=]() { return GetTotalCurrent(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Voltage", [=]() { return GetVoltage(); }, nullptr);
+  builder.AddDoubleProperty(
+      "TotalCurrent", [=]() { return GetTotalCurrent(); }, nullptr);
 }

wpilibc/src/main/native/cpp/Preferences.cpp

diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 6014775..8c1eb87 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -58,36 +58,66 @@
   entry.SetPersistent();
 }
 
+void Preferences::InitString(wpi::StringRef key, wpi::StringRef value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultString(value);
+}
+
 void Preferences::PutInt(wpi::StringRef key, int value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitInt(wpi::StringRef key, int value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 void Preferences::PutDouble(wpi::StringRef key, double value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitDouble(wpi::StringRef key, double value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 void Preferences::PutFloat(wpi::StringRef key, float value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitFloat(wpi::StringRef key, float value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 void Preferences::PutBoolean(wpi::StringRef key, bool value) {
   auto entry = m_table->GetEntry(key);
   entry.SetBoolean(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitBoolean(wpi::StringRef key, bool value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultBoolean(value);
+}
+
 void Preferences::PutLong(wpi::StringRef key, int64_t value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitLong(wpi::StringRef key, int64_t value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 bool Preferences::ContainsKey(wpi::StringRef key) {
   return m_table->ContainsKey(key);
 }

wpilibc/src/main/native/cpp/RobotController.cpp

diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
index b1d7608..b33479f 100644
--- a/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/RobotController.h"
 
 #include <hal/CAN.h>
-#include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Power.h>
 

wpilibc/src/main/native/cpp/RobotDrive.cpp

diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp
index 5235fa2..9eafe0c 100644
--- a/wpilibc/src/main/native/cpp/RobotDrive.cpp
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -282,7 +282,7 @@
   double yIn = y;
   // Negate y for the joystick.
   yIn = -yIn;
-  // Compenstate for gyro angle.
+  // Compensate for gyro angle.
   RotateVector(xIn, yIn, gyroAngle);
 
   double wheelSpeeds[kMaxNumberOfMotors];

wpilibc/src/main/native/cpp/ScopedTracer.cpp

diff --git a/wpilibc/src/main/native/cpp/ScopedTracer.cpp b/wpilibc/src/main/native/cpp/ScopedTracer.cpp
new file mode 100644
index 0000000..2024a65
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ScopedTracer.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ScopedTracer.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+ScopedTracer::ScopedTracer(wpi::Twine name, wpi::raw_ostream& os)
+    : m_name(name.str()), m_os(os) {
+  m_tracer.ResetTimer();
+}
+
+ScopedTracer::~ScopedTracer() {
+  m_tracer.AddEpoch(m_name);
+  m_tracer.PrintEpochs(m_os);
+}

wpilibc/src/main/native/cpp/SensorUtil.cpp

diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp
index 8c02f1b..f86c72c 100644
--- a/wpilibc/src/main/native/cpp/SensorUtil.cpp
+++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 #include <hal/AnalogInput.h>
 #include <hal/AnalogOutput.h>
 #include <hal/DIO.h>
-#include <hal/FRCUsageReporting.h>
 #include <hal/PDP.h>
 #include <hal/PWM.h>
 #include <hal/Ports.h>
@@ -21,6 +20,7 @@
 
 const int SensorUtil::kDigitalChannels = HAL_GetNumDigitalChannels();
 const int SensorUtil::kAnalogInputs = HAL_GetNumAnalogInputs();
+const int SensorUtil::kAnalogOutputs = HAL_GetNumAnalogOutputs();
 const int SensorUtil::kSolenoidChannels = HAL_GetNumSolenoidChannels();
 const int SensorUtil::kSolenoidModules = HAL_GetNumPCMModules();
 const int SensorUtil::kPwmChannels = HAL_GetNumPWMChannels();

wpilibc/src/main/native/cpp/Servo.cpp

diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
index 4b6856a..5edcebc 100644
--- a/wpilibc/src/main/native/cpp/Servo.cpp
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -57,8 +57,8 @@
 
 void Servo::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Servo");
-  builder.AddDoubleProperty("Value", [=]() { return Get(); },
-                            [=](double value) { Set(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
 }
 
 double Servo::GetServoAngleRange() const {

wpilibc/src/main/native/cpp/Solenoid.cpp

diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp
index e0bde3c..b5abf20 100644
--- a/wpilibc/src/main/native/cpp/Solenoid.cpp
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -56,6 +56,7 @@
 
 void Solenoid::Set(bool on) {
   if (StatusIsFatal()) return;
+
   int32_t status = 0;
   HAL_SetSolenoid(m_solenoidHandle, on, &status);
   wpi_setHALError(status);
@@ -63,12 +64,16 @@
 
 bool Solenoid::Get() const {
   if (StatusIsFatal()) return false;
+
   int32_t status = 0;
   bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
   wpi_setHALError(status);
+
   return value;
 }
 
+void Solenoid::Toggle() { Set(!Get()); }
+
 bool Solenoid::IsBlackListed() const {
   int value = GetPCMSolenoidBlackList(m_moduleNumber) & (1 << m_channel);
   return (value != 0);
@@ -93,6 +98,6 @@
   builder.SetSmartDashboardType("Solenoid");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { Set(false); });
-  builder.AddBooleanProperty("Value", [=]() { return Get(); },
-                             [=](bool value) { Set(value); });
+  builder.AddBooleanProperty(
+      "Value", [=]() { return Get(); }, [=](bool value) { Set(value); });
 }

wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp

diff --git a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
index a807afc..8fea1b2 100644
--- a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
+++ b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,9 +8,27 @@
 #include "frc/SpeedControllerGroup.h"
 
 #include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
+// Can't use a delegated constructor here because of an MSVC bug.
+// https://developercommunity.visualstudio.com/content/problem/583/compiler-bug-with-delegating-a-constructor.html
+
+SpeedControllerGroup::SpeedControllerGroup(
+    std::vector<std::reference_wrapper<SpeedController>>&& speedControllers)
+    : m_speedControllers(std::move(speedControllers)) {
+  Initialize();
+}
+
+void SpeedControllerGroup::Initialize() {
+  for (auto& speedController : m_speedControllers)
+    SendableRegistry::GetInstance().AddChild(this, &speedController.get());
+  static int instances = 0;
+  ++instances;
+  SendableRegistry::GetInstance().Add(this, "SpeedControllerGroup", instances);
+}
+
 void SpeedControllerGroup::Set(double speed) {
   for (auto speedController : m_speedControllers) {
     speedController.get().Set(m_isInverted ? -speed : speed);
@@ -48,6 +66,6 @@
   builder.SetSmartDashboardType("Speed Controller");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { StopMotor(); });
-  builder.AddDoubleProperty("Value", [=]() { return Get(); },
-                            [=](double value) { Set(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
 }

wpilibc/src/main/native/cpp/TimedRobot.cpp

diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
index a9358dd..74e658a 100644
--- a/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,24 +24,44 @@
 void TimedRobot::StartCompetition() {
   RobotInit();
 
+  if constexpr (IsSimulation()) {
+    SimulationInit();
+  }
+
   // Tell the DS that the robot is ready to be enabled
   HAL_ObserveUserProgramStarting();
 
-  m_expirationTime = units::second_t{Timer::GetFPGATimestamp()} + m_period;
-  UpdateAlarm();
-
   // Loop forever, calling the appropriate mode-dependent function
   while (true) {
+    // We don't have to check there's an element in the queue first because
+    // there's always at least one (the constructor adds one). It's reenqueued
+    // at the end of the loop.
+    auto callback = m_callbacks.pop();
+
     int32_t status = 0;
+    HAL_UpdateNotifierAlarm(
+        m_notifier, static_cast<uint64_t>(callback.expirationTime * 1e6),
+        &status);
+    wpi_setHALError(status);
+
     uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
     if (curTime == 0 || status != 0) break;
 
-    m_expirationTime += m_period;
+    callback.func();
 
-    UpdateAlarm();
+    callback.expirationTime += callback.period;
+    m_callbacks.push(std::move(callback));
 
-    // Call callback
-    LoopFunc();
+    // Process all other callbacks that are ready to run
+    while (static_cast<uint64_t>(m_callbacks.top().expirationTime * 1e6) <=
+           curTime) {
+      callback = m_callbacks.pop();
+
+      callback.func();
+
+      callback.expirationTime += callback.period;
+      m_callbacks.push(std::move(callback));
+    }
   }
 }
 
@@ -57,6 +77,9 @@
 TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
 
 TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
+  m_startTime = frc2::Timer::GetFPGATimestamp();
+  AddPeriodic([=] { LoopFunc(); }, period);
+
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
   wpi_setHALError(status);
@@ -75,9 +98,7 @@
   HAL_CleanNotifier(m_notifier, &status);
 }
 
-void TimedRobot::UpdateAlarm() {
-  int32_t status = 0;
-  HAL_UpdateNotifierAlarm(
-      m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
-  wpi_setHALError(status);
+void TimedRobot::AddPeriodic(std::function<void()> callback,
+                             units::second_t period, units::second_t offset) {
+  m_callbacks.emplace(callback, m_startTime, period, offset);
 }

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

diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp
index c91bc13..b428e03 100644
--- a/wpilibc/src/main/native/cpp/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,13 +7,7 @@
 
 #include "frc/Timer.h"
 
-#include <chrono>
-#include <thread>
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/DriverStation.h"
-#include "frc/RobotController.h"
+#include <units/time.h>
 
 namespace frc {
 
@@ -25,8 +19,6 @@
 
 using namespace frc;
 
-const double Timer::kRolloverTime = frc2::Timer::kRolloverTime.to<double>();
-
 Timer::Timer() { Reset(); }
 
 double Timer::Get() const { return m_timer.Get().to<double>(); }

wpilibc/src/main/native/cpp/Tracer.cpp

diff --git a/wpilibc/src/main/native/cpp/Tracer.cpp b/wpilibc/src/main/native/cpp/Tracer.cpp
new file mode 100644
index 0000000..af5177a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Tracer.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Tracer.h"
+
+#include <wpi/Format.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+
+Tracer::Tracer() { ResetTimer(); }
+
+void Tracer::ResetTimer() { m_startTime = hal::fpga_clock::now(); }
+
+void Tracer::ClearEpochs() {
+  ResetTimer();
+  m_epochs.clear();
+}
+
+void Tracer::AddEpoch(wpi::StringRef epochName) {
+  auto currentTime = hal::fpga_clock::now();
+  m_epochs[epochName] = currentTime - m_startTime;
+  m_startTime = currentTime;
+}
+
+void Tracer::PrintEpochs() {
+  wpi::SmallString<128> buf;
+  wpi::raw_svector_ostream os(buf);
+  PrintEpochs(os);
+  if (!buf.empty()) DriverStation::ReportWarning(buf);
+}
+
+void Tracer::PrintEpochs(wpi::raw_ostream& os) {
+  using std::chrono::duration_cast;
+  using std::chrono::microseconds;
+
+  auto now = hal::fpga_clock::now();
+  if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+    m_lastEpochsPrintTime = now;
+    for (const auto& epoch : m_epochs) {
+      os << '\t' << epoch.getKey() << ": "
+         << wpi::format(
+                "%.6f",
+                duration_cast<microseconds>(epoch.getValue()).count() / 1.0e6)
+         << "s\n";
+    }
+  }
+}

wpilibc/src/main/native/cpp/Ultrasonic.cpp

diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
index fcd016e..dbd0d13 100644
--- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,6 +9,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/Counter.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalOutput.h"
@@ -177,8 +178,8 @@
 
 void Ultrasonic::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Ultrasonic");
-  builder.AddDoubleProperty("Value", [=]() { return GetRangeInches(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetRangeInches(); }, nullptr);
 }
 
 void Ultrasonic::Initialize() {

wpilibc/src/main/native/cpp/Utility.cpp

diff --git a/wpilibc/src/main/native/cpp/Utility.cpp b/wpilibc/src/main/native/cpp/Utility.cpp
index 11f6234..4b69cc8 100644
--- a/wpilibc/src/main/native/cpp/Utility.cpp
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,19 +12,13 @@
 #include <execinfo.h>
 #endif
 
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-
+#include <frc/Base.h>
 #include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
 #include <wpi/Path.h>
 #include <wpi/SmallString.h>
 #include <wpi/StackTrace.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/ErrorBase.h"
-
 using namespace frc;
 
 bool wpi_assert_impl(bool conditionValue, const wpi::Twine& conditionText,

wpilibc/src/main/native/cpp/Watchdog.cpp

diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp
index e5be4fa..81c7cc8 100644
--- a/wpilibc/src/main/native/cpp/Watchdog.cpp
+++ b/wpilibc/src/main/native/cpp/Watchdog.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,16 +7,24 @@
 
 #include "frc/Watchdog.h"
 
+#include <atomic>
+
+#include <hal/Notifier.h>
 #include <wpi/Format.h>
-#include <wpi/PriorityQueue.h>
+#include <wpi/SmallString.h>
+#include <wpi/priority_queue.h>
 #include <wpi/raw_ostream.h>
 
+#include "frc/DriverStation.h"
+#include "frc2/Timer.h"
+
 using namespace frc;
 
-constexpr std::chrono::milliseconds Watchdog::kMinPrintPeriod;
-
-class Watchdog::Thread : public wpi::SafeThread {
+class Watchdog::Impl {
  public:
+  Impl();
+  ~Impl();
+
   template <typename T>
   struct DerefGreater {
     constexpr bool operator()(const T& lhs, const T& rhs) const {
@@ -24,56 +32,96 @@
     }
   };
 
-  wpi::PriorityQueue<Watchdog*, std::vector<Watchdog*>, DerefGreater<Watchdog*>>
+  wpi::mutex m_mutex;
+  std::atomic<HAL_NotifierHandle> m_notifier;
+  wpi::priority_queue<Watchdog*, std::vector<Watchdog*>,
+                      DerefGreater<Watchdog*>>
       m_watchdogs;
 
+  void UpdateAlarm();
+
  private:
-  void Main() override;
+  void Main();
+
+  std::thread m_thread;
 };
 
-void Watchdog::Thread::Main() {
-  std::unique_lock lock(m_mutex);
+Watchdog::Impl::Impl() {
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  wpi_setGlobalHALError(status);
+  HAL_SetNotifierName(m_notifier, "Watchdog", &status);
 
-  while (m_active) {
-    if (m_watchdogs.size() > 0) {
-      if (m_cond.wait_until(lock, m_watchdogs.top()->m_expirationTime) ==
-          std::cv_status::timeout) {
-        if (m_watchdogs.size() == 0 ||
-            m_watchdogs.top()->m_expirationTime > hal::fpga_clock::now()) {
-          continue;
-        }
+  m_thread = std::thread([=] { Main(); });
+}
 
-        // If the condition variable timed out, that means a Watchdog timeout
-        // has occurred, so call its timeout function.
-        auto watchdog = m_watchdogs.top();
-        m_watchdogs.pop();
+Watchdog::Impl::~Impl() {
+  int32_t status = 0;
+  // atomically set handle to 0, then clean
+  HAL_NotifierHandle handle = m_notifier.exchange(0);
+  HAL_StopNotifier(handle, &status);
+  wpi_setGlobalHALError(status);
 
-        auto now = hal::fpga_clock::now();
-        if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
-          watchdog->m_lastTimeoutPrintTime = now;
-          if (!watchdog->m_suppressTimeoutMessage) {
-            wpi::outs() << "Watchdog not fed within "
-                        << wpi::format("%.6f",
-                                       watchdog->m_timeout.count() / 1.0e9)
-                        << "s\n";
-          }
-        }
+  // Join the thread to ensure the handler has exited.
+  if (m_thread.joinable()) m_thread.join();
 
-        // Set expiration flag before calling the callback so any manipulation
-        // of the flag in the callback (e.g., calling Disable()) isn't
-        // clobbered.
-        watchdog->m_isExpired = true;
+  HAL_CleanNotifier(handle, &status);
+}
 
-        lock.unlock();
-        watchdog->m_callback();
-        lock.lock();
+void Watchdog::Impl::UpdateAlarm() {
+  int32_t status = 0;
+  // Return if we are being destructed, or were not created successfully
+  auto notifier = m_notifier.load();
+  if (notifier == 0) return;
+  if (m_watchdogs.empty())
+    HAL_CancelNotifierAlarm(notifier, &status);
+  else
+    HAL_UpdateNotifierAlarm(
+        notifier,
+        static_cast<uint64_t>(m_watchdogs.top()->m_expirationTime.to<double>() *
+                              1e6),
+        &status);
+  wpi_setGlobalHALError(status);
+}
+
+void Watchdog::Impl::Main() {
+  for (;;) {
+    int32_t status = 0;
+    HAL_NotifierHandle notifier = m_notifier.load();
+    if (notifier == 0) break;
+    uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+    if (curTime == 0 || status != 0) break;
+
+    std::unique_lock lock(m_mutex);
+
+    if (m_watchdogs.empty()) continue;
+
+    // If the condition variable timed out, that means a Watchdog timeout
+    // has occurred, so call its timeout function.
+    auto watchdog = m_watchdogs.pop();
+
+    units::second_t now{curTime * 1e-6};
+    if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
+      watchdog->m_lastTimeoutPrintTime = now;
+      if (!watchdog->m_suppressTimeoutMessage) {
+        wpi::SmallString<128> buf;
+        wpi::raw_svector_ostream err(buf);
+        err << "Watchdog not fed within "
+            << wpi::format("%.6f", watchdog->m_timeout.to<double>()) << "s\n";
+        frc::DriverStation::ReportWarning(err.str());
       }
-      // Otherwise, a Watchdog removed itself from the queue (it notifies the
-      // scheduler of this) or a spurious wakeup occurred, so just rewait with
-      // the soonest watchdog timeout.
-    } else {
-      m_cond.wait(lock, [&] { return m_watchdogs.size() > 0 || !m_active; });
     }
+
+    // Set expiration flag before calling the callback so any manipulation
+    // of the flag in the callback (e.g., calling Disable()) isn't
+    // clobbered.
+    watchdog->m_isExpired = true;
+
+    lock.unlock();
+    watchdog->m_callback();
+    lock.lock();
+
+    UpdateAlarm();
   }
 }
 
@@ -81,12 +129,32 @@
     : Watchdog(units::second_t{timeout}, callback) {}
 
 Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
-    : m_timeout(timeout), m_callback(callback), m_owner(&GetThreadOwner()) {}
+    : m_timeout(timeout), m_callback(callback), m_impl(GetImpl()) {}
 
 Watchdog::~Watchdog() { Disable(); }
 
+Watchdog::Watchdog(Watchdog&& rhs) { *this = std::move(rhs); }
+
+Watchdog& Watchdog::operator=(Watchdog&& rhs) {
+  m_impl = rhs.m_impl;
+  std::scoped_lock lock(m_impl->m_mutex);
+  m_startTime = rhs.m_startTime;
+  m_timeout = rhs.m_timeout;
+  m_expirationTime = rhs.m_expirationTime;
+  m_callback = std::move(rhs.m_callback);
+  m_lastTimeoutPrintTime = rhs.m_lastTimeoutPrintTime;
+  m_suppressTimeoutMessage = rhs.m_suppressTimeoutMessage;
+  m_tracer = std::move(rhs.m_tracer);
+  m_isExpired = rhs.m_isExpired;
+  if (m_expirationTime != 0_s) {
+    m_impl->m_watchdogs.remove(&rhs);
+    m_impl->m_watchdogs.emplace(this);
+  }
+  return *this;
+}
+
 double Watchdog::GetTime() const {
-  return (hal::fpga_clock::now() - m_startTime).count() / 1.0e6;
+  return (frc2::Timer::GetFPGATimestamp() - m_startTime).to<double>();
 }
 
 void Watchdog::SetTimeout(double timeout) {
@@ -94,100 +162,69 @@
 }
 
 void Watchdog::SetTimeout(units::second_t timeout) {
-  using std::chrono::duration_cast;
-  using std::chrono::microseconds;
+  m_startTime = frc2::Timer::GetFPGATimestamp();
+  m_tracer.ClearEpochs();
 
-  m_startTime = hal::fpga_clock::now();
-  m_epochs.clear();
-
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-  if (!thr) return;
-
+  std::scoped_lock lock(m_impl->m_mutex);
   m_timeout = timeout;
   m_isExpired = false;
 
-  thr->m_watchdogs.remove(this);
-  m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
-  thr->m_watchdogs.emplace(this);
-  thr->m_cond.notify_all();
+  m_impl->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  m_impl->m_watchdogs.emplace(this);
+  m_impl->UpdateAlarm();
 }
 
 double Watchdog::GetTimeout() const {
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-
-  return m_timeout.count() / 1.0e9;
+  std::scoped_lock lock(m_impl->m_mutex);
+  return m_timeout.to<double>();
 }
 
 bool Watchdog::IsExpired() const {
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-
+  std::scoped_lock lock(m_impl->m_mutex);
   return m_isExpired;
 }
 
 void Watchdog::AddEpoch(wpi::StringRef epochName) {
-  auto currentTime = hal::fpga_clock::now();
-  m_epochs[epochName] = currentTime - m_startTime;
-  m_startTime = currentTime;
+  m_tracer.AddEpoch(epochName);
 }
 
-void Watchdog::PrintEpochs() {
-  auto now = hal::fpga_clock::now();
-  if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
-    m_lastEpochsPrintTime = now;
-    for (const auto& epoch : m_epochs) {
-      wpi::outs() << '\t' << epoch.getKey() << ": "
-                  << wpi::format("%.6f", epoch.getValue().count() / 1.0e6)
-                  << "s\n";
-    }
-  }
-}
+void Watchdog::PrintEpochs() { m_tracer.PrintEpochs(); }
 
 void Watchdog::Reset() { Enable(); }
 
 void Watchdog::Enable() {
-  using std::chrono::duration_cast;
-  using std::chrono::microseconds;
+  m_startTime = frc2::Timer::GetFPGATimestamp();
+  m_tracer.ClearEpochs();
 
-  m_startTime = hal::fpga_clock::now();
-  m_epochs.clear();
-
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-  if (!thr) return;
-
+  std::scoped_lock lock(m_impl->m_mutex);
   m_isExpired = false;
 
-  thr->m_watchdogs.remove(this);
-  m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
-  thr->m_watchdogs.emplace(this);
-  thr->m_cond.notify_all();
+  m_impl->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  m_impl->m_watchdogs.emplace(this);
+  m_impl->UpdateAlarm();
 }
 
 void Watchdog::Disable() {
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-  if (!thr) return;
+  std::scoped_lock lock(m_impl->m_mutex);
 
-  thr->m_watchdogs.remove(this);
-  thr->m_cond.notify_all();
+  if (m_expirationTime != 0_s) {
+    m_impl->m_watchdogs.remove(this);
+    m_expirationTime = 0_s;
+    m_impl->UpdateAlarm();
+  }
 }
 
 void Watchdog::SuppressTimeoutMessage(bool suppress) {
   m_suppressTimeoutMessage = suppress;
 }
 
-bool Watchdog::operator>(const Watchdog& rhs) {
+bool Watchdog::operator>(const Watchdog& rhs) const {
   return m_expirationTime > rhs.m_expirationTime;
 }
 
-wpi::SafeThreadOwner<Watchdog::Thread>& Watchdog::GetThreadOwner() {
-  static wpi::SafeThreadOwner<Thread> inst = [] {
-    wpi::SafeThreadOwner<Watchdog::Thread> inst;
-    inst.Start();
-    return inst;
-  }();
-  return inst;
+Watchdog::Impl* Watchdog::GetImpl() {
+  static Impl inst;
+  return &inst;
 }

wpilibc/src/main/native/cpp/XboxController.cpp

diff --git a/wpilibc/src/main/native/cpp/XboxController.cpp b/wpilibc/src/main/native/cpp/XboxController.cpp
index b294257..de77624 100644
--- a/wpilibc/src/main/native/cpp/XboxController.cpp
+++ b/wpilibc/src/main/native/cpp/XboxController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,25 +17,25 @@
 
 double XboxController::GetX(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(0);
+    return GetRawAxis(static_cast<int>(Axis::kLeftX));
   } else {
-    return GetRawAxis(4);
+    return GetRawAxis(static_cast<int>(Axis::kRightX));
   }
 }
 
 double XboxController::GetY(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(1);
+    return GetRawAxis(static_cast<int>(Axis::kLeftY));
   } else {
-    return GetRawAxis(5);
+    return GetRawAxis(static_cast<int>(Axis::kRightY));
   }
 }
 
 double XboxController::GetTriggerAxis(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(2);
+    return GetRawAxis(static_cast<int>(Axis::kLeftTrigger));
   } else {
-    return GetRawAxis(3);
+    return GetRawAxis(static_cast<int>(Axis::kRightTrigger));
   }
 }
 

wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp

diff --git a/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
new file mode 100644
index 0000000..7482e22
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/HolonomicDriveController.h"
+
+#include <units/angular_velocity.h>
+
+using namespace frc;
+
+HolonomicDriveController::HolonomicDriveController(
+    const frc2::PIDController& xController,
+    const frc2::PIDController& yController,
+    const ProfiledPIDController<units::radian>& thetaController)
+    : m_xController(xController),
+      m_yController(yController),
+      m_thetaController(thetaController) {}
+
+bool HolonomicDriveController::AtReference() const {
+  const auto& eTranslate = m_poseError.Translation();
+  const auto& eRotate = m_poseError.Rotation();
+  const auto& tolTranslate = m_poseTolerance.Translation();
+  const auto& tolRotate = m_poseTolerance.Rotation();
+  return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+         units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+         units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
+  m_poseTolerance = tolerance;
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+    const Pose2d& currentPose, const Pose2d& poseRef,
+    units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+  // Calculate feedforward velocities (field-relative)
+  auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
+  auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
+  auto thetaFF = units::radians_per_second_t(m_thetaController.Calculate(
+      currentPose.Rotation().Radians(), angleRef.Radians()));
+
+  m_poseError = poseRef.RelativeTo(currentPose);
+
+  if (!m_enabled) {
+    return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
+                                                  currentPose.Rotation());
+  }
+
+  // Calculate feedback velocities (based on position error).
+  auto xFeedback = units::meters_per_second_t(m_xController.Calculate(
+      currentPose.X().to<double>(), poseRef.X().to<double>()));
+  auto yFeedback = units::meters_per_second_t(m_yController.Calculate(
+      currentPose.Y().to<double>(), poseRef.Y().to<double>()));
+
+  // Return next output.
+  return ChassisSpeeds::FromFieldRelativeSpeeds(
+      xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+    const Pose2d& currentPose, const Trajectory::State& desiredState,
+    const Rotation2d& angleRef) {
+  return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+                   angleRef);
+}
+
+void HolonomicDriveController::SetEnabled(bool enabled) { m_enabled = enabled; }

wpilibc/src/main/native/cpp/controller/PIDController.cpp

diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 3abd4ba..7b9b623 100644
--- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/controller/ControllerUtil.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -26,6 +27,12 @@
   frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
 }
 
+void PIDController::SetPID(double Kp, double Ki, double Kd) {
+  m_Kp = Kp;
+  m_Ki = Ki;
+  m_Kd = Kd;
+}
+
 void PIDController::SetP(double Kp) { m_Kp = Kp; }
 
 void PIDController::SetI(double Ki) { m_Ki = Ki; }
@@ -42,13 +49,7 @@
   return units::second_t(m_period);
 }
 
-void PIDController::SetSetpoint(double setpoint) {
-  if (m_maximumInput > m_minimumInput) {
-    m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
-  } else {
-    m_setpoint = setpoint;
-  }
-}
+void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
 
 double PIDController::GetSetpoint() const { return m_setpoint; }
 
@@ -60,11 +61,14 @@
 void PIDController::EnableContinuousInput(double minimumInput,
                                           double maximumInput) {
   m_continuous = true;
-  SetInputRange(minimumInput, maximumInput);
+  m_minimumInput = minimumInput;
+  m_maximumInput = maximumInput;
 }
 
 void PIDController::DisableContinuousInput() { m_continuous = false; }
 
+bool PIDController::IsContinuousInputEnabled() const { return m_continuous; }
+
 void PIDController::SetIntegratorRange(double minimumIntegral,
                                        double maximumIntegral) {
   m_minimumIntegral = minimumIntegral;
@@ -77,15 +81,20 @@
   m_velocityTolerance = velocityTolerance;
 }
 
-double PIDController::GetPositionError() const {
-  return GetContinuousError(m_positionError);
-}
+double PIDController::GetPositionError() const { return m_positionError; }
 
 double PIDController::GetVelocityError() const { return m_velocityError; }
 
 double PIDController::Calculate(double measurement) {
   m_prevError = m_positionError;
-  m_positionError = GetContinuousError(m_setpoint - measurement);
+
+  if (m_continuous) {
+    m_positionError = frc::GetModulusError<double>(
+        m_setpoint, measurement, m_minimumInput, m_maximumInput);
+  } else {
+    m_positionError = m_setpoint - measurement;
+  }
+
   m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
 
   if (m_Ki != 0) {
@@ -110,38 +119,13 @@
 
 void PIDController::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("PIDController");
-  builder.AddDoubleProperty("p", [this] { return GetP(); },
-                            [this](double value) { SetP(value); });
-  builder.AddDoubleProperty("i", [this] { return GetI(); },
-                            [this](double value) { SetI(value); });
-  builder.AddDoubleProperty("d", [this] { return GetD(); },
-                            [this](double value) { SetD(value); });
-  builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); },
-                            [this](double value) { SetSetpoint(value); });
-}
-
-double PIDController::GetContinuousError(double error) const {
-  if (m_continuous && m_inputRange > 0) {
-    error = std::fmod(error, m_inputRange);
-    if (std::abs(error) > m_inputRange / 2) {
-      if (error > 0) {
-        return error - m_inputRange;
-      } else {
-        return error + m_inputRange;
-      }
-    }
-  }
-
-  return error;
-}
-
-void PIDController::SetInputRange(double minimumInput, double maximumInput) {
-  m_minimumInput = minimumInput;
-  m_maximumInput = maximumInput;
-  m_inputRange = maximumInput - minimumInput;
-
-  // Clamp setpoint to new input range
-  if (m_maximumInput > m_minimumInput) {
-    m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput);
-  }
+  builder.AddDoubleProperty(
+      "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
+  builder.AddDoubleProperty(
+      "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
+  builder.AddDoubleProperty(
+      "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
+  builder.AddDoubleProperty(
+      "setpoint", [this] { return GetSetpoint(); },
+      [this](double value) { SetSetpoint(value); });
 }

wpilibc/src/main/native/cpp/controller/RamseteController.cpp

diff --git a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
index c55c2e8..aad6937 100644
--- a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,6 +9,8 @@
 
 #include <cmath>
 
+#include <units/math.h>
+
 using namespace frc;
 
 /**
@@ -45,11 +47,15 @@
     const Pose2d& currentPose, const Pose2d& poseRef,
     units::meters_per_second_t linearVelocityRef,
     units::radians_per_second_t angularVelocityRef) {
+  if (!m_enabled) {
+    return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+  }
+
   m_poseError = poseRef.RelativeTo(currentPose);
 
   // Aliases for equation readability
-  double eX = m_poseError.Translation().X().to<double>();
-  double eY = m_poseError.Translation().Y().to<double>();
+  double eX = m_poseError.X().to<double>();
+  double eY = m_poseError.Y().to<double>();
   double eTheta = m_poseError.Rotation().Radians().to<double>();
   double vRef = linearVelocityRef.to<double>();
   double omegaRef = angularVelocityRef.to<double>();
@@ -68,3 +74,5 @@
   return Calculate(currentPose, desiredState.pose, desiredState.velocity,
                    desiredState.velocity * desiredState.curvature);
 }
+
+void RamseteController::SetEnabled(bool enabled) { m_enabled = enabled; }

wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp

diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index 2d30a2b..ce9aa05 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -216,9 +216,9 @@
   builder.SetSmartDashboardType("DifferentialDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
-  builder.AddDoubleProperty("Left Motor Speed",
-                            [=]() { return m_leftMotor->Get(); },
-                            [=](double value) { m_leftMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Left Motor Speed", [=]() { return m_leftMotor->Get(); },
+      [=](double value) { m_leftMotor->Set(value); });
   builder.AddDoubleProperty(
       "Right Motor Speed",
       [=]() { return m_rightMotor->Get() * m_rightSideInvertMultiplier; },

wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp

diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index e03951b..983ce6a 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -107,13 +107,13 @@
   builder.SetSmartDashboardType("KilloughDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
-  builder.AddDoubleProperty("Left Motor Speed",
-                            [=]() { return m_leftMotor->Get(); },
-                            [=](double value) { m_leftMotor->Set(value); });
-  builder.AddDoubleProperty("Right Motor Speed",
-                            [=]() { return m_rightMotor->Get(); },
-                            [=](double value) { m_rightMotor->Set(value); });
-  builder.AddDoubleProperty("Back Motor Speed",
-                            [=]() { return m_backMotor->Get(); },
-                            [=](double value) { m_backMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Left Motor Speed", [=]() { return m_leftMotor->Get(); },
+      [=](double value) { m_leftMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Right Motor Speed", [=]() { return m_rightMotor->Get(); },
+      [=](double value) { m_rightMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Back Motor Speed", [=]() { return m_backMotor->Get(); },
+      [=](double value) { m_backMotor->Set(value); });
 }

wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp

diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index 0102d6a..cb11d8a 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -120,9 +120,9 @@
       [=](double value) {
         m_frontRightMotor->Set(value * m_rightSideInvertMultiplier);
       });
-  builder.AddDoubleProperty("Rear Left Motor Speed",
-                            [=]() { return m_rearLeftMotor->Get(); },
-                            [=](double value) { m_rearLeftMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Rear Left Motor Speed", [=]() { return m_rearLeftMotor->Get(); },
+      [=](double value) { m_rearLeftMotor->Set(value); });
   builder.AddDoubleProperty(
       "Rear Right Motor Speed",
       [=]() { return m_rearRightMotor->Get() * m_rightSideInvertMultiplier; },

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

diff --git a/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
index 76721bc..36da4c6 100644
--- a/wpilibc/src/main/native/cpp/frc2/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,8 +10,6 @@
 #include <chrono>
 #include <thread>
 
-#include <hal/FRCUsageReporting.h>
-
 #include "frc/DriverStation.h"
 #include "frc/RobotController.h"
 
@@ -36,9 +34,6 @@
 
 using namespace frc2;
 
-// for compatibility with msvc12--see C2864
-const units::second_t Timer::kRolloverTime = units::second_t((1ll << 32) / 1e6);
-
 Timer::Timer() { Reset(); }
 
 Timer::Timer(const Timer& rhs)
@@ -77,13 +72,6 @@
 
   std::scoped_lock lock(m_mutex);
   if (m_running) {
-    // If the current time is before the start time, then the FPGA clock rolled
-    // over. Compensate by adding the ~71 minutes that it takes to roll over to
-    // the current time.
-    if (currentTime < m_startTime) {
-      currentTime += kRolloverTime;
-    }
-
     result = (currentTime - m_startTime) + m_accumulatedTime;
   } else {
     result = m_accumulatedTime;
@@ -116,15 +104,22 @@
   }
 }
 
+bool Timer::HasElapsed(units::second_t period) const { return Get() > period; }
+
 bool Timer::HasPeriodPassed(units::second_t period) {
+  return AdvanceIfElapsed(period);
+}
+
+bool Timer::AdvanceIfElapsed(units::second_t period) {
   if (Get() > period) {
     std::scoped_lock lock(m_mutex);
     // Advance the start time by the period.
     m_startTime += period;
     // Don't set it to the current time... we want to avoid drift.
     return true;
+  } else {
+    return false;
   }
-  return false;
 }
 
 units::second_t Timer::GetFPGATimestamp() {

wpilibc/src/main/native/cpp/geometry/Pose2d.cpp

diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
deleted file mode 100644
index 1754830..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/geometry/Pose2d.h"
-
-#include <cmath>
-
-#include <wpi/json.h>
-
-using namespace frc;
-
-Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
-
-Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
-    : m_translation(x, y), m_rotation(rotation) {}
-
-Pose2d Pose2d::operator+(const Transform2d& other) const {
-  return TransformBy(other);
-}
-
-Pose2d& Pose2d::operator+=(const Transform2d& other) {
-  m_translation += other.Translation().RotateBy(m_rotation);
-  m_rotation += other.Rotation();
-  return *this;
-}
-
-Transform2d Pose2d::operator-(const Pose2d& other) const {
-  const auto pose = this->RelativeTo(other);
-  return Transform2d(pose.Translation(), pose.Rotation());
-}
-
-bool Pose2d::operator==(const Pose2d& other) const {
-  return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Pose2d::operator!=(const Pose2d& other) const {
-  return !operator==(other);
-}
-
-Pose2d Pose2d::TransformBy(const Transform2d& other) const {
-  return {m_translation + (other.Translation().RotateBy(m_rotation)),
-          m_rotation + other.Rotation()};
-}
-
-Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
-  const Transform2d transform{other, *this};
-  return {transform.Translation(), transform.Rotation()};
-}
-
-Pose2d Pose2d::Exp(const Twist2d& twist) const {
-  const auto dx = twist.dx;
-  const auto dy = twist.dy;
-  const auto dtheta = twist.dtheta.to<double>();
-
-  const auto sinTheta = std::sin(dtheta);
-  const auto cosTheta = std::cos(dtheta);
-
-  double s, c;
-  if (std::abs(dtheta) < 1E-9) {
-    s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
-    c = 0.5 * dtheta;
-  } else {
-    s = sinTheta / dtheta;
-    c = (1 - cosTheta) / dtheta;
-  }
-
-  const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
-                              Rotation2d{cosTheta, sinTheta}};
-
-  return *this + transform;
-}
-
-Twist2d Pose2d::Log(const Pose2d& end) const {
-  const auto transform = end.RelativeTo(*this);
-  const auto dtheta = transform.Rotation().Radians().to<double>();
-  const auto halfDtheta = dtheta / 2.0;
-
-  const auto cosMinusOne = transform.Rotation().Cos() - 1;
-
-  double halfThetaByTanOfHalfDtheta;
-
-  if (std::abs(cosMinusOne) < 1E-9) {
-    halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
-  } else {
-    halfThetaByTanOfHalfDtheta =
-        -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
-  }
-
-  const Translation2d translationPart =
-      transform.Translation().RotateBy(
-          {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
-      std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
-
-  return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
-}
-
-void frc::to_json(wpi::json& json, const Pose2d& pose) {
-  json = wpi::json{{"translation", pose.Translation()},
-                   {"rotation", pose.Rotation()}};
-}
-
-void frc::from_json(const wpi::json& json, Pose2d& pose) {
-  pose = Pose2d{json.at("translation").get<Translation2d>(),
-                json.at("rotation").get<Rotation2d>()};
-}

wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp

diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
deleted file mode 100644
index a3c4bea..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/geometry/Rotation2d.h"
-
-#include <cmath>
-
-#include <wpi/json.h>
-
-using namespace frc;
-
-Rotation2d::Rotation2d(units::radian_t value)
-    : m_value(value),
-      m_cos(units::math::cos(value)),
-      m_sin(units::math::sin(value)) {}
-
-Rotation2d::Rotation2d(double x, double y) {
-  const auto magnitude = std::hypot(x, y);
-  if (magnitude > 1e-6) {
-    m_sin = y / magnitude;
-    m_cos = x / magnitude;
-  } else {
-    m_sin = 0.0;
-    m_cos = 1.0;
-  }
-  m_value = units::radian_t(std::atan2(m_sin, m_cos));
-}
-
-Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
-  return RotateBy(other);
-}
-
-Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
-  double cos = Cos() * other.Cos() - Sin() * other.Sin();
-  double sin = Cos() * other.Sin() + Sin() * other.Cos();
-  m_cos = cos;
-  m_sin = sin;
-  m_value = units::radian_t(std::atan2(m_sin, m_cos));
-  return *this;
-}
-
-Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
-  return *this + -other;
-}
-
-Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
-  *this += -other;
-  return *this;
-}
-
-Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
-
-Rotation2d Rotation2d::operator*(double scalar) const {
-  return Rotation2d(m_value * scalar);
-}
-
-bool Rotation2d::operator==(const Rotation2d& other) const {
-  return units::math::abs(m_value - other.m_value) < 1E-9_rad;
-}
-
-bool Rotation2d::operator!=(const Rotation2d& other) const {
-  return !operator==(other);
-}
-
-Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
-  return {Cos() * other.Cos() - Sin() * other.Sin(),
-          Cos() * other.Sin() + Sin() * other.Cos()};
-}
-
-void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
-  json = wpi::json{{"radians", rotation.Radians().to<double>()}};
-}
-
-void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
-  rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
-}

wpilibc/src/main/native/cpp/geometry/Transform2d.cpp

diff --git a/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
deleted file mode 100644
index 04f0419..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/geometry/Transform2d.h"
-
-#include "frc/geometry/Pose2d.h"
-
-using namespace frc;
-
-Transform2d::Transform2d(Pose2d initial, Pose2d final) {
-  // We are rotating the difference between the translations
-  // using a clockwise rotation matrix. This transforms the global
-  // delta into a local delta (relative to the initial pose).
-  m_translation = (final.Translation() - initial.Translation())
-                      .RotateBy(-initial.Rotation());
-
-  m_rotation = final.Rotation() - initial.Rotation();
-}
-
-Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
-
-bool Transform2d::operator==(const Transform2d& other) const {
-  return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Transform2d::operator!=(const Transform2d& other) const {
-  return !operator==(other);
-}

wpilibc/src/main/native/cpp/geometry/Translation2d.cpp

diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
deleted file mode 100644
index c3db7e3..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/geometry/Translation2d.h"
-
-#include <wpi/json.h>
-
-using namespace frc;
-
-Translation2d::Translation2d(units::meter_t x, units::meter_t y)
-    : m_x(x), m_y(y) {}
-
-units::meter_t Translation2d::Distance(const Translation2d& other) const {
-  return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
-}
-
-units::meter_t Translation2d::Norm() const {
-  return units::math::hypot(m_x, m_y);
-}
-
-Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
-  return {m_x * other.Cos() - m_y * other.Sin(),
-          m_x * other.Sin() + m_y * other.Cos()};
-}
-
-Translation2d Translation2d::operator+(const Translation2d& other) const {
-  return {X() + other.X(), Y() + other.Y()};
-}
-
-Translation2d& Translation2d::operator+=(const Translation2d& other) {
-  m_x += other.m_x;
-  m_y += other.m_y;
-  return *this;
-}
-
-Translation2d Translation2d::operator-(const Translation2d& other) const {
-  return *this + -other;
-}
-
-Translation2d& Translation2d::operator-=(const Translation2d& other) {
-  *this += -other;
-  return *this;
-}
-
-Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
-
-Translation2d Translation2d::operator*(double scalar) const {
-  return {scalar * m_x, scalar * m_y};
-}
-
-Translation2d& Translation2d::operator*=(double scalar) {
-  m_x *= scalar;
-  m_y *= scalar;
-  return *this;
-}
-
-Translation2d Translation2d::operator/(double scalar) const {
-  return *this * (1.0 / scalar);
-}
-
-bool Translation2d::operator==(const Translation2d& other) const {
-  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
-         units::math::abs(m_y - other.m_y) < 1E-9_m;
-}
-
-bool Translation2d::operator!=(const Translation2d& other) const {
-  return !operator==(other);
-}
-
-Translation2d& Translation2d::operator/=(double scalar) {
-  *this *= (1.0 / scalar);
-  return *this;
-}
-
-void frc::to_json(wpi::json& json, const Translation2d& translation) {
-  json = wpi::json{{"x", translation.X().to<double>()},
-                   {"y", translation.Y().to<double>()}};
-}
-
-void frc::from_json(const wpi::json& json, Translation2d& translation) {
-  translation = Translation2d{units::meter_t{json.at("x").get<double>()},
-                              units::meter_t{json.at("y").get<double>()}};
-}

wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp

diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
deleted file mode 100644
index 07a8e3b..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/DifferentialDriveOdometry.h"
-
-#include <hal/FRCUsageReporting.h>
-
-using namespace frc;
-
-DifferentialDriveOdometry::DifferentialDriveOdometry(
-    const Rotation2d& gyroAngle, const Pose2d& initialPose)
-    : m_pose(initialPose) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_DifferentialDrive);
-}
-
-const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
-                                                units::meter_t leftDistance,
-                                                units::meter_t rightDistance) {
-  auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
-  auto deltaRightDistance = rightDistance - m_prevRightDistance;
-
-  m_prevLeftDistance = leftDistance;
-  m_prevRightDistance = rightDistance;
-
-  auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto newPose = m_pose.Exp(
-      {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}

wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp

diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
deleted file mode 100644
index 5b13f0e..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
-
-using namespace frc;
-
-void DifferentialDriveWheelSpeeds::Normalize(
-    units::meters_per_second_t attainableMaxSpeed) {
-  auto realMaxSpeed =
-      units::math::max(units::math::abs(left), units::math::abs(right));
-
-  if (realMaxSpeed > attainableMaxSpeed) {
-    left = left / realMaxSpeed * attainableMaxSpeed;
-    right = right / realMaxSpeed * attainableMaxSpeed;
-  }
-}

wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp

diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
deleted file mode 100644
index cf6ebb5..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/MecanumDriveKinematics.h"
-
-using namespace frc;
-
-MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
-    const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
-  // We have a new center of rotation. We need to compute the matrix again.
-  if (centerOfRotation != m_previousCoR) {
-    auto fl = m_frontLeftWheel - centerOfRotation;
-    auto fr = m_frontRightWheel - centerOfRotation;
-    auto rl = m_rearLeftWheel - centerOfRotation;
-    auto rr = m_rearRightWheel - centerOfRotation;
-
-    SetInverseKinematics(fl, fr, rl, rr);
-
-    m_previousCoR = centerOfRotation;
-  }
-
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
-
-  Eigen::Matrix<double, 4, 1> wheelsMatrix =
-      m_inverseKinematics * chassisSpeedsVector;
-
-  MecanumDriveWheelSpeeds wheelSpeeds;
-  wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
-  wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
-  wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
-  wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
-  return wheelSpeeds;
-}
-
-ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
-    const MecanumDriveWheelSpeeds& wheelSpeeds) {
-  Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
-  // clang-format off
-  wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
-                       wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
-  // clang-format on
-
-  Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(wheelSpeedsMatrix);
-
-  return {units::meters_per_second_t{chassisSpeedsVector(0)},
-          units::meters_per_second_t{chassisSpeedsVector(1)},
-          units::radians_per_second_t{chassisSpeedsVector(2)}};
-}
-
-void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
-                                                  Translation2d fr,
-                                                  Translation2d rl,
-                                                  Translation2d rr) {
-  // clang-format off
-  m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
-                         1,  1, (fr.X() - fr.Y()).template to<double>(),
-                         1,  1, (rl.X() - rl.Y()).template to<double>(),
-                         1, -1, (-(rr.X() + rr.Y())).template to<double>();
-  // clang-format on
-  m_inverseKinematics /= std::sqrt(2);
-}

wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp

diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
deleted file mode 100644
index 3843483..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/MecanumDriveOdometry.h"
-
-#include <hal/FRCUsageReporting.h>
-
-using namespace frc;
-
-MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
-                                           const Rotation2d& gyroAngle,
-                                           const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_MecanumDrive);
-}
-
-const Pose2d& MecanumDriveOdometry::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    MecanumDriveWheelSpeeds wheelSpeeds) {
-  units::second_t deltaTime =
-      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
-  m_previousTime = currentTime;
-
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
-  static_cast<void>(dtheta);
-
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}

wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp

diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
deleted file mode 100644
index 1641ba8..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
-
-#include <algorithm>
-#include <array>
-#include <cmath>
-
-using namespace frc;
-
-void MecanumDriveWheelSpeeds::Normalize(
-    units::meters_per_second_t attainableMaxSpeed) {
-  std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
-                                                        rearLeft, rearRight};
-  units::meters_per_second_t realMaxSpeed = *std::max_element(
-      wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
-        return units::math::abs(a) < units::math::abs(b);
-      });
-
-  if (realMaxSpeed > attainableMaxSpeed) {
-    for (int i = 0; i < 4; ++i) {
-      wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
-    }
-    frontLeft = wheelSpeeds[0];
-    frontRight = wheelSpeeds[1];
-    rearLeft = wheelSpeeds[2];
-    rearRight = wheelSpeeds[3];
-  }
-}

wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp

diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index 10996e4..517ecf7 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2012-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -104,10 +104,12 @@
   // Force table generation now to make sure everything is defined
   UpdateValuesUnsafe();
   if (enabled) {
+    if (this->enabled) this->enabled();
   } else {
     m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
       cbdata.builder.StopLiveWindowMode();
     });
+    if (this->disabled) this->disabled();
   }
   m_impl->enabledEntry.SetBoolean(enabled);
 }

wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp

diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
index b6f8ce9..663cc19 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -30,7 +30,7 @@
 }  // namespace detail
 
 void SendableCameraWrapper::InitSendable(SendableBuilder& builder) {
-  builder.AddStringProperty(".ShuffleboardURI", [this] { return m_uri; },
-                            nullptr);
+  builder.AddStringProperty(
+      ".ShuffleboardURI", [this] { return m_uri; }, nullptr);
 }
 }  // namespace frc

wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp

diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
index bb9dc9e..d73f635 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -47,7 +47,7 @@
   wpi::SmallVector<char, 16> storage;
   auto titleRef = title.toStringRef(storage);
   if (m_layouts.count(titleRef) == 0) {
-    auto layout = std::make_unique<ShuffleboardLayout>(*this, type, titleRef);
+    auto layout = std::make_unique<ShuffleboardLayout>(*this, titleRef, type);
     auto ptr = layout.get();
     m_components.emplace_back(std::move(layout));
     m_layouts.insert(std::make_pair(titleRef, ptr));

wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp

diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index 9717a8e..3316b3e 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,7 @@
 #include <hal/FRCUsageReporting.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpi/SmallVector.h>
 #include <wpi/StringMap.h>
 
 #include "frc/shuffleboard/Shuffleboard.h"
@@ -43,7 +44,7 @@
 
 void ShuffleboardInstance::Update() {
   if (m_impl->tabsChanged) {
-    std::vector<std::string> tabTitles;
+    wpi::SmallVector<std::string, 16> tabTitles;
     for (auto& entry : m_impl->tabs) {
       tabTitles.emplace_back(entry.second.GetTitle());
     }

wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp

diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
index 8418b77..1cbfb80 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,11 +10,11 @@
 using namespace frc;
 
 ShuffleboardLayout::ShuffleboardLayout(ShuffleboardContainer& parent,
-                                       const wpi::Twine& name,
+                                       const wpi::Twine& title,
                                        const wpi::Twine& type)
-    : ShuffleboardValue(type),
-      ShuffleboardComponent(parent, type, name),
-      ShuffleboardContainer(name) {
+    : ShuffleboardValue(title),
+      ShuffleboardComponent(parent, title, type),
+      ShuffleboardContainer(title) {
   m_isLayout = true;
 }
 

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

diff --git a/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
new file mode 100644
index 0000000..74a5180
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/ADXRS450_GyroSim.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/ADXRS450_Gyro.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+ADXRS450_GyroSim::ADXRS450_GyroSim(const frc::ADXRS450_Gyro& gyro) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << "ADXRS450_Gyro" << '[' << gyro.GetPort() << ']';
+  frc::sim::SimDeviceSim deviceSim{fullname.c_str()};
+  m_simAngle = deviceSim.GetDouble("Angle");
+  m_simRate = deviceSim.GetDouble("Rate");
+}
+
+void ADXRS450_GyroSim::SetAngle(units::degree_t angle) {
+  m_simAngle.Set(angle.to<double>());
+}
+
+void ADXRS450_GyroSim::SetRate(units::degrees_per_second_t rate) {
+  m_simRate.Set(rate.to<double>());
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp b/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
new file mode 100644
index 0000000..743c51d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/AddressableLEDSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/AddressableLEDData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+AddressableLEDSim::AddressableLEDSim() : m_index{0} {}
+
+AddressableLEDSim::AddressableLEDSim(const AddressableLED& addressableLED)
+    : m_index{0} {}
+
+AddressableLEDSim AddressableLEDSim::CreateForChannel(int pwmChannel) {
+  int index = HALSIM_FindAddressableLEDForChannel(pwmChannel);
+  if (index < 0)
+    throw std::out_of_range("no addressable LED found for PWM channel");
+  return AddressableLEDSim{index};
+}
+
+AddressableLEDSim AddressableLEDSim::CreateForIndex(int index) {
+  return AddressableLEDSim{index};
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDInitializedCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AddressableLEDSim::GetInitialized() const {
+  return HALSIM_GetAddressableLEDInitialized(m_index);
+}
+
+void AddressableLEDSim::SetInitialized(bool initialized) {
+  HALSIM_SetAddressableLEDInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterOutputPortCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDOutputPortCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDOutputPortCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AddressableLEDSim::GetOutputPort() const {
+  return HALSIM_GetAddressableLEDOutputPort(m_index);
+}
+
+void AddressableLEDSim::SetOutputPort(int outputPort) {
+  HALSIM_SetAddressableLEDOutputPort(m_index, outputPort);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterLengthCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDLengthCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDLengthCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AddressableLEDSim::GetLength() const {
+  return HALSIM_GetAddressableLEDLength(m_index);
+}
+
+void AddressableLEDSim::SetLength(int length) {
+  HALSIM_SetAddressableLEDLength(m_index, length);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterRunningCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDRunningCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDRunningCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AddressableLEDSim::GetRunning() const {
+  return HALSIM_GetAddressableLEDRunning(m_index);
+}
+
+void AddressableLEDSim::SetRunning(bool running) {
+  HALSIM_SetAddressableLEDRunning(m_index, running);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterDataCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDDataCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDDataCallback(
+      m_index, &ConstBufferCallbackStoreThunk, store.get()));
+  return store;
+}
+
+int AddressableLEDSim::GetData(struct HAL_AddressableLEDData* data) const {
+  return HALSIM_GetAddressableLEDData(m_index, data);
+}
+
+void AddressableLEDSim::SetData(struct HAL_AddressableLEDData* data,
+                                int length) {
+  HALSIM_SetAddressableLEDData(m_index, data, length);
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
new file mode 100644
index 0000000..5fee737
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/AnalogEncoderSim.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/AnalogEncoder.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+AnalogEncoderSim::AnalogEncoderSim(const frc::AnalogEncoder& encoder) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << "AnalogEncoder" << '[' << encoder.GetChannel() << ']';
+  frc::sim::SimDeviceSim deviceSim{fullname.c_str()};
+  m_positionSim = deviceSim.GetDouble("Position");
+}
+
+void AnalogEncoderSim::SetPosition(frc::Rotation2d angle) {
+  SetTurns(angle.Degrees());
+}
+
+void AnalogEncoderSim::SetTurns(units::turn_t turns) {
+  m_positionSim.Set(turns.to<double>());
+}
+
+units::turn_t AnalogEncoderSim::GetTurns() {
+  return units::turn_t{m_positionSim.Get()};
+}
+
+frc::Rotation2d AnalogEncoderSim::GetPosition() {
+  units::radian_t rads = GetTurns();
+  return frc::Rotation2d{rads};
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
new file mode 100644
index 0000000..b7cab6b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/AnalogGyroSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AnalogGyroData.h>
+
+#include "frc/AnalogGyro.h"
+#include "frc/AnalogInput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogGyroSim::AnalogGyroSim(const AnalogGyro& gyro)
+    : m_index{gyro.GetAnalogInput()->GetChannel()} {}
+
+AnalogGyroSim::AnalogGyroSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> AnalogGyroSim::RegisterAngleCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogGyroAngleCallback);
+  store->SetUid(HALSIM_RegisterAnalogGyroAngleCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogGyroSim::GetAngle() const {
+  return HALSIM_GetAnalogGyroAngle(m_index);
+}
+
+void AnalogGyroSim::SetAngle(double angle) {
+  HALSIM_SetAnalogGyroAngle(m_index, angle);
+}
+
+std::unique_ptr<CallbackStore> AnalogGyroSim::RegisterRateCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogGyroRateCallback);
+  store->SetUid(HALSIM_RegisterAnalogGyroRateCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogGyroSim::GetRate() const {
+  return HALSIM_GetAnalogGyroRate(m_index);
+}
+
+void AnalogGyroSim::SetRate(double rate) {
+  HALSIM_SetAnalogGyroRate(m_index, rate);
+}
+
+std::unique_ptr<CallbackStore> AnalogGyroSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogGyroInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogGyroInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogGyroSim::GetInitialized() const {
+  return HALSIM_GetAnalogGyroInitialized(m_index);
+}
+
+void AnalogGyroSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogGyroInitialized(m_index, initialized);
+}
+
+void AnalogGyroSim::ResetData() { HALSIM_ResetAnalogGyroData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
new file mode 100644
index 0000000..057a188
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/AnalogInputSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AnalogInData.h>
+
+#include "frc/AnalogInput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogInputSim::AnalogInputSim(const AnalogInput& analogInput)
+    : m_index{analogInput.GetChannel()} {}
+
+AnalogInputSim::AnalogInputSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogInInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogInputSim::GetInitialized() const {
+  return HALSIM_GetAnalogInInitialized(m_index);
+}
+
+void AnalogInputSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogInInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterAverageBitsCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAverageBitsCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAverageBitsCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetAverageBits() const {
+  return HALSIM_GetAnalogInAverageBits(m_index);
+}
+
+void AnalogInputSim::SetAverageBits(int averageBits) {
+  HALSIM_SetAnalogInAverageBits(m_index, averageBits);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterOversampleBitsCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInOversampleBitsCallback);
+  store->SetUid(HALSIM_RegisterAnalogInOversampleBitsCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetOversampleBits() const {
+  return HALSIM_GetAnalogInOversampleBits(m_index);
+}
+
+void AnalogInputSim::SetOversampleBits(int oversampleBits) {
+  HALSIM_SetAnalogInOversampleBits(m_index, oversampleBits);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInVoltageCallback);
+  store->SetUid(HALSIM_RegisterAnalogInVoltageCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogInputSim::GetVoltage() const {
+  return HALSIM_GetAnalogInVoltage(m_index);
+}
+
+void AnalogInputSim::SetVoltage(double voltage) {
+  HALSIM_SetAnalogInVoltage(m_index, voltage);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogInputSim::RegisterAccumulatorInitializedCallback(NotifyCallback callback,
+                                                       bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelAnalogInAccumulatorInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogInputSim::GetAccumulatorInitialized() const {
+  return HALSIM_GetAnalogInAccumulatorInitialized(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorInitialized(bool accumulatorInitialized) {
+  HALSIM_SetAnalogInAccumulatorInitialized(m_index, accumulatorInitialized);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterAccumulatorValueCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorValueCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorValueCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int64_t AnalogInputSim::GetAccumulatorValue() const {
+  return HALSIM_GetAnalogInAccumulatorValue(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorValue(int64_t accumulatorValue) {
+  HALSIM_SetAnalogInAccumulatorValue(m_index, accumulatorValue);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterAccumulatorCountCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCountCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorCountCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int64_t AnalogInputSim::GetAccumulatorCount() const {
+  return HALSIM_GetAnalogInAccumulatorCount(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorCount(int64_t accumulatorCount) {
+  HALSIM_SetAnalogInAccumulatorCount(m_index, accumulatorCount);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogInputSim::RegisterAccumulatorCenterCallback(NotifyCallback callback,
+                                                  bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCenterCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorCenterCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetAccumulatorCenter() const {
+  return HALSIM_GetAnalogInAccumulatorCenter(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorCenter(int accumulatorCenter) {
+  HALSIM_SetAnalogInAccumulatorCenter(m_index, accumulatorCenter);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogInputSim::RegisterAccumulatorDeadbandCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorDeadbandCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorDeadbandCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetAccumulatorDeadband() const {
+  return HALSIM_GetAnalogInAccumulatorDeadband(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorDeadband(int accumulatorDeadband) {
+  HALSIM_SetAnalogInAccumulatorDeadband(m_index, accumulatorDeadband);
+}
+
+void AnalogInputSim::ResetData() { HALSIM_ResetAnalogInData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
new file mode 100644
index 0000000..4de9082
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/AnalogOutputSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AnalogOutData.h>
+
+#include "frc/AnalogOutput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogOutputSim::AnalogOutputSim(const AnalogOutput& analogOutput)
+    : m_index{analogOutput.GetChannel()} {}
+
+AnalogOutputSim::AnalogOutputSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> AnalogOutputSim::RegisterVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogOutVoltageCallback);
+  store->SetUid(HALSIM_RegisterAnalogOutVoltageCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogOutputSim::GetVoltage() const {
+  return HALSIM_GetAnalogOutVoltage(m_index);
+}
+
+void AnalogOutputSim::SetVoltage(double voltage) {
+  HALSIM_SetAnalogOutVoltage(m_index, voltage);
+}
+
+std::unique_ptr<CallbackStore> AnalogOutputSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogOutInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogOutInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogOutputSim::GetInitialized() const {
+  return HALSIM_GetAnalogOutInitialized(m_index);
+}
+
+void AnalogOutputSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogOutInitialized(m_index, initialized);
+}
+
+void AnalogOutputSim::ResetData() { HALSIM_ResetAnalogOutData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
new file mode 100644
index 0000000..3325827
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/AnalogTriggerSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/AnalogTriggerData.h>
+
+#include "frc/AnalogTrigger.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogTriggerSim::AnalogTriggerSim(const AnalogTrigger& analogTrigger)
+    : m_index{analogTrigger.GetIndex()} {}
+
+AnalogTriggerSim AnalogTriggerSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindAnalogTriggerForChannel(channel);
+  if (index < 0) throw std::out_of_range("no analog trigger found for channel");
+  return AnalogTriggerSim{index};
+}
+
+AnalogTriggerSim AnalogTriggerSim::CreateForIndex(int index) {
+  return AnalogTriggerSim{index};
+}
+
+std::unique_ptr<CallbackStore> AnalogTriggerSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogTriggerInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogTriggerInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogTriggerSim::GetInitialized() const {
+  return HALSIM_GetAnalogTriggerInitialized(m_index);
+}
+
+void AnalogTriggerSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogTriggerInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogTriggerSim::RegisterTriggerLowerBoundCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback);
+  store->SetUid(HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogTriggerSim::GetTriggerLowerBound() const {
+  return HALSIM_GetAnalogTriggerTriggerLowerBound(m_index);
+}
+
+void AnalogTriggerSim::SetTriggerLowerBound(double triggerLowerBound) {
+  HALSIM_SetAnalogTriggerTriggerLowerBound(m_index, triggerLowerBound);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogTriggerSim::RegisterTriggerUpperBoundCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback);
+  store->SetUid(HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogTriggerSim::GetTriggerUpperBound() const {
+  return HALSIM_GetAnalogTriggerTriggerUpperBound(m_index);
+}
+
+void AnalogTriggerSim::SetTriggerUpperBound(double triggerUpperBound) {
+  HALSIM_SetAnalogTriggerTriggerUpperBound(m_index, triggerUpperBound);
+}
+
+void AnalogTriggerSim::ResetData() { HALSIM_ResetAnalogTriggerData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
new file mode 100644
index 0000000..601ee6e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/BuiltInAccelerometerSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AccelerometerData.h>
+
+#include "frc/BuiltInAccelerometer.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+BuiltInAccelerometerSim::BuiltInAccelerometerSim() : m_index{0} {}
+
+BuiltInAccelerometerSim::BuiltInAccelerometerSim(const BuiltInAccelerometer&)
+    : m_index{0} {}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterActiveCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerActiveCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerActiveCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool BuiltInAccelerometerSim::GetActive() const {
+  return HALSIM_GetAccelerometerActive(m_index);
+}
+
+void BuiltInAccelerometerSim::SetActive(bool active) {
+  HALSIM_SetAccelerometerActive(m_index, active);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterRangeCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerRangeCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerRangeCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+HAL_AccelerometerRange BuiltInAccelerometerSim::GetRange() const {
+  return HALSIM_GetAccelerometerRange(m_index);
+}
+
+void BuiltInAccelerometerSim::SetRange(HAL_AccelerometerRange range) {
+  HALSIM_SetAccelerometerRange(m_index, range);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterXCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerXCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerXCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double BuiltInAccelerometerSim::GetX() const {
+  return HALSIM_GetAccelerometerX(m_index);
+}
+
+void BuiltInAccelerometerSim::SetX(double x) {
+  HALSIM_SetAccelerometerX(m_index, x);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterYCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerYCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerYCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double BuiltInAccelerometerSim::GetY() const {
+  return HALSIM_GetAccelerometerY(m_index);
+}
+
+void BuiltInAccelerometerSim::SetY(double y) {
+  HALSIM_SetAccelerometerY(m_index, y);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterZCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerZCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerZCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double BuiltInAccelerometerSim::GetZ() const {
+  return HALSIM_GetAccelerometerZ(m_index);
+}
+
+void BuiltInAccelerometerSim::SetZ(double z) {
+  HALSIM_SetAccelerometerZ(m_index, z);
+}
+
+void BuiltInAccelerometerSim::ResetData() {
+  HALSIM_ResetAccelerometerData(m_index);
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp b/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
new file mode 100644
index 0000000..5c38372
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/CallbackStore.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+void frc::sim::CallbackStoreThunk(const char* name, void* param,
+                                  const HAL_Value* value) {
+  reinterpret_cast<CallbackStore*>(param)->callback(name, value);
+}
+
+void frc::sim::ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                             const unsigned char* buffer,
+                                             unsigned int count) {
+  reinterpret_cast<CallbackStore*>(param)->constBufferCallback(name, buffer,
+                                                               count);
+}
+
+CallbackStore::CallbackStore(int32_t i, NotifyCallback cb,
+                             CancelCallbackNoIndexFunc ccf)
+    : index(i), callback(cb), cancelType(NoIndex) {
+  this->ccnif = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t u, NotifyCallback cb,
+                             CancelCallbackFunc ccf)
+    : index(i), uid(u), callback(cb), cancelType(Normal) {
+  this->ccf = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t c, int32_t u, NotifyCallback cb,
+                             CancelCallbackChannelFunc ccf)
+    : index(i), channel(c), uid(u), callback(cb), cancelType(Channel) {
+  this->cccf = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, ConstBufferCallback cb,
+                             CancelCallbackNoIndexFunc ccf)
+    : index(i), constBufferCallback(cb), cancelType(NoIndex) {
+  this->ccnif = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t u, ConstBufferCallback cb,
+                             CancelCallbackFunc ccf)
+    : index(i), uid(u), constBufferCallback(cb), cancelType(Normal) {
+  this->ccf = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t c, int32_t u,
+                             ConstBufferCallback cb,
+                             CancelCallbackChannelFunc ccf)
+    : index(i),
+      channel(c),
+      uid(u),
+      constBufferCallback(cb),
+      cancelType(Channel) {
+  this->cccf = ccf;
+}
+
+CallbackStore::~CallbackStore() {
+  switch (cancelType) {
+    case Normal:
+      ccf(index, uid);
+      break;
+    case Channel:
+      cccf(index, channel, uid);
+      break;
+    case NoIndex:
+      ccnif(uid);
+      break;
+  }
+}
+
+void CallbackStore::SetUid(int32_t uid) { this->uid = uid; }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp b/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
new file mode 100644
index 0000000..9330735
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/DIOSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/DIOData.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DIOSim::DIOSim(const DigitalInput& input) : m_index{input.GetChannel()} {}
+
+DIOSim::DIOSim(const DigitalOutput& output) : m_index{output.GetChannel()} {}
+
+DIOSim::DIOSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOInitializedCallback);
+  store->SetUid(HALSIM_RegisterDIOInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DIOSim::GetInitialized() const {
+  return HALSIM_GetDIOInitialized(m_index);
+}
+
+void DIOSim::SetInitialized(bool initialized) {
+  HALSIM_SetDIOInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterValueCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(m_index, -1, callback,
+                                               &HALSIM_CancelDIOValueCallback);
+  store->SetUid(HALSIM_RegisterDIOValueCallback(m_index, &CallbackStoreThunk,
+                                                store.get(), initialNotify));
+  return store;
+}
+
+bool DIOSim::GetValue() const { return HALSIM_GetDIOValue(m_index); }
+
+void DIOSim::SetValue(bool value) { HALSIM_SetDIOValue(m_index, value); }
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterPulseLengthCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOPulseLengthCallback);
+  store->SetUid(HALSIM_RegisterDIOPulseLengthCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DIOSim::GetPulseLength() const {
+  return HALSIM_GetDIOPulseLength(m_index);
+}
+
+void DIOSim::SetPulseLength(double pulseLength) {
+  HALSIM_SetDIOPulseLength(m_index, pulseLength);
+}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterIsInputCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOIsInputCallback);
+  store->SetUid(HALSIM_RegisterDIOIsInputCallback(m_index, &CallbackStoreThunk,
+                                                  store.get(), initialNotify));
+  return store;
+}
+
+bool DIOSim::GetIsInput() const { return HALSIM_GetDIOIsInput(m_index); }
+
+void DIOSim::SetIsInput(bool isInput) {
+  HALSIM_SetDIOIsInput(m_index, isInput);
+}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterFilterIndexCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOFilterIndexCallback);
+  store->SetUid(HALSIM_RegisterDIOFilterIndexCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int DIOSim::GetFilterIndex() const { return HALSIM_GetDIOFilterIndex(m_index); }
+
+void DIOSim::SetFilterIndex(int filterIndex) {
+  HALSIM_SetDIOFilterIndex(m_index, filterIndex);
+}
+
+void DIOSim::ResetData() { HALSIM_ResetDIOData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
new file mode 100644
index 0000000..51ed94a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/DifferentialDrivetrainSim.h"
+
+#include <frc/system/plant/LinearSystemId.h>
+
+#include "frc/system/RungeKutta.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DifferentialDrivetrainSim::DifferentialDrivetrainSim(
+    const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth,
+    DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius)
+    : m_plant(plant),
+      m_rb(trackWidth / 2.0),
+      m_wheelRadius(wheelRadius),
+      m_motor(driveMotor),
+      m_originalGearing(gearRatio),
+      m_currentGearing(gearRatio) {
+  m_x.setZero();
+  m_u.setZero();
+}
+
+DifferentialDrivetrainSim::DifferentialDrivetrainSim(
+    frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
+    units::kilogram_t mass, units::meter_t wheelRadius,
+    units::meter_t trackWidth)
+    : DifferentialDrivetrainSim(
+          frc::LinearSystemId::DrivetrainVelocitySystem(
+              driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
+          trackWidth, driveMotor, gearing, wheelRadius) {}
+
+void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
+                                          units::volt_t rightVoltage) {
+  m_u << leftVoltage.to<double>(), rightVoltage.to<double>();
+}
+
+void DifferentialDrivetrainSim::SetGearing(double newGearing) {
+  m_currentGearing = newGearing;
+}
+
+void DifferentialDrivetrainSim::Update(units::second_t dt) {
+  m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x,
+                   m_u, dt);
+}
+
+double DifferentialDrivetrainSim::GetState(int state) const {
+  return m_x(state);
+}
+
+double DifferentialDrivetrainSim::GetGearing() const {
+  return m_currentGearing;
+}
+
+Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetState() const {
+  return m_x;
+}
+
+Rotation2d DifferentialDrivetrainSim::GetHeading() const {
+  return Rotation2d(units::radian_t(m_x(State::kHeading)));
+}
+
+Pose2d DifferentialDrivetrainSim::GetPose() const {
+  return Pose2d(units::meter_t(m_x(State::kX)), units::meter_t(m_x(State::kY)),
+                Rotation2d(units::radian_t(m_x(State::kHeading))));
+}
+
+units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
+  auto loadIleft =
+      m_motor.Current(units::radians_per_second_t(m_x(State::kLeftVelocity) *
+                                                  m_currentGearing /
+                                                  m_wheelRadius.to<double>()),
+                      units::volt_t(m_u(0))) *
+      wpi::sgn(m_u(0));
+
+  auto loadIRight =
+      m_motor.Current(units::radians_per_second_t(m_x(State::kRightVelocity) *
+                                                  m_currentGearing /
+                                                  m_wheelRadius.to<double>()),
+                      units::volt_t(m_u(1))) *
+      wpi::sgn(m_u(1));
+
+  return loadIleft + loadIRight;
+}
+
+void DifferentialDrivetrainSim::SetState(
+    const Eigen::Matrix<double, 7, 1>& state) {
+  m_x = state;
+}
+
+void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
+  m_x(State::kX) = pose.X().to<double>();
+  m_x(State::kY) = pose.Y().to<double>();
+  m_x(State::kHeading) = pose.Rotation().Radians().to<double>();
+  m_x(State::kLeftPosition) = 0;
+  m_x(State::kRightPosition) = 0;
+}
+
+Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
+    const Eigen::Matrix<double, 7, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
+  // Because G^2 can be factored out of A, we can divide by the old ratio
+  // squared and multiply by the new ratio squared to get a new drivetrain
+  // model.
+  Eigen::Matrix<double, 4, 2> B;
+  B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing /
+                        m_originalGearing / m_originalGearing;
+  B.block<2, 2>(2, 0).setZero();
+
+  // Because G can be factored out of B, we can divide by the old ratio and
+  // multiply by the new ratio to get a new drivetrain model.
+  Eigen::Matrix<double, 4, 4> A;
+  A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing;
+
+  A.block<2, 2>(2, 0).setIdentity();
+  A.block<4, 2>(0, 2).setZero();
+
+  double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
+
+  Eigen::Matrix<double, 7, 1> xdot;
+  xdot(0) = v * std::cos(x(State::kHeading));
+  xdot(1) = v * std::sin(x(State::kHeading));
+  xdot(2) =
+      ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / (2.0 * m_rb))
+          .to<double>();
+  xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u;
+  return xdot;
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
new file mode 100644
index 0000000..2ee55ce
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/DigitalPWMSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/DigitalPWMData.h>
+
+#include "frc/DigitalOutput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DigitalPWMSim::DigitalPWMSim(const DigitalOutput& digitalOutput)
+    : m_index{digitalOutput.GetChannel()} {}
+
+DigitalPWMSim DigitalPWMSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindDigitalPWMForChannel(channel);
+  if (index < 0) throw std::out_of_range("no digital PWM found for channel");
+  return DigitalPWMSim{index};
+}
+
+DigitalPWMSim DigitalPWMSim::CreateForIndex(int index) {
+  return DigitalPWMSim{index};
+}
+
+std::unique_ptr<CallbackStore> DigitalPWMSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDigitalPWMInitializedCallback);
+  store->SetUid(HALSIM_RegisterDigitalPWMInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DigitalPWMSim::GetInitialized() const {
+  return HALSIM_GetDigitalPWMInitialized(m_index);
+}
+
+void DigitalPWMSim::SetInitialized(bool initialized) {
+  HALSIM_SetDigitalPWMInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> DigitalPWMSim::RegisterDutyCycleCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDigitalPWMDutyCycleCallback);
+  store->SetUid(HALSIM_RegisterDigitalPWMDutyCycleCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DigitalPWMSim::GetDutyCycle() const {
+  return HALSIM_GetDigitalPWMDutyCycle(m_index);
+}
+
+void DigitalPWMSim::SetDutyCycle(double dutyCycle) {
+  HALSIM_SetDigitalPWMDutyCycle(m_index, dutyCycle);
+}
+
+std::unique_ptr<CallbackStore> DigitalPWMSim::RegisterPinCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDigitalPWMPinCallback);
+  store->SetUid(HALSIM_RegisterDigitalPWMPinCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int DigitalPWMSim::GetPin() const { return HALSIM_GetDigitalPWMPin(m_index); }
+
+void DigitalPWMSim::SetPin(int pin) { HALSIM_SetDigitalPWMPin(m_index, pin); }
+
+void DigitalPWMSim::ResetData() { HALSIM_ResetDigitalPWMData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
new file mode 100644
index 0000000..8c0bf70
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
@@ -0,0 +1,255 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/DriverStationSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/DriverStationData.h>
+#include <hal/simulation/MockHooks.h>
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterEnabledCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationEnabledCallback);
+  store->SetUid(HALSIM_RegisterDriverStationEnabledCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetEnabled() { return HALSIM_GetDriverStationEnabled(); }
+
+void DriverStationSim::SetEnabled(bool enabled) {
+  HALSIM_SetDriverStationEnabled(enabled);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterAutonomousCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationAutonomousCallback);
+  store->SetUid(HALSIM_RegisterDriverStationAutonomousCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetAutonomous() {
+  return HALSIM_GetDriverStationAutonomous();
+}
+
+void DriverStationSim::SetAutonomous(bool autonomous) {
+  HALSIM_SetDriverStationAutonomous(autonomous);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterTestCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationTestCallback);
+  store->SetUid(HALSIM_RegisterDriverStationTestCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetTest() { return HALSIM_GetDriverStationTest(); }
+
+void DriverStationSim::SetTest(bool test) { HALSIM_SetDriverStationTest(test); }
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterEStopCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationEStopCallback);
+  store->SetUid(HALSIM_RegisterDriverStationEStopCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetEStop() { return HALSIM_GetDriverStationEStop(); }
+
+void DriverStationSim::SetEStop(bool eStop) {
+  HALSIM_SetDriverStationEStop(eStop);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterFmsAttachedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationFmsAttachedCallback);
+  store->SetUid(HALSIM_RegisterDriverStationFmsAttachedCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetFmsAttached() {
+  return HALSIM_GetDriverStationFmsAttached();
+}
+
+void DriverStationSim::SetFmsAttached(bool fmsAttached) {
+  HALSIM_SetDriverStationFmsAttached(fmsAttached);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterDsAttachedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationDsAttachedCallback);
+  store->SetUid(HALSIM_RegisterDriverStationDsAttachedCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetDsAttached() {
+  return HALSIM_GetDriverStationDsAttached();
+}
+
+void DriverStationSim::SetDsAttached(bool dsAttached) {
+  HALSIM_SetDriverStationDsAttached(dsAttached);
+}
+
+std::unique_ptr<CallbackStore>
+DriverStationSim::RegisterAllianceStationIdCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationAllianceStationIdCallback);
+  store->SetUid(HALSIM_RegisterDriverStationAllianceStationIdCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+HAL_AllianceStationID DriverStationSim::GetAllianceStationId() {
+  return HALSIM_GetDriverStationAllianceStationId();
+}
+
+void DriverStationSim::SetAllianceStationId(
+    HAL_AllianceStationID allianceStationId) {
+  HALSIM_SetDriverStationAllianceStationId(allianceStationId);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterMatchTimeCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationMatchTimeCallback);
+  store->SetUid(HALSIM_RegisterDriverStationMatchTimeCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DriverStationSim::GetMatchTime() {
+  return HALSIM_GetDriverStationMatchTime();
+}
+
+void DriverStationSim::SetMatchTime(double matchTime) {
+  HALSIM_SetDriverStationMatchTime(matchTime);
+}
+
+void DriverStationSim::NotifyNewData() {
+  HALSIM_NotifyDriverStationNewData();
+  DriverStation::GetInstance().WaitForData();
+}
+
+void DriverStationSim::SetSendError(bool shouldSend) {
+  if (shouldSend) {
+    HALSIM_SetSendError(nullptr);
+  } else {
+    HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
+                           HAL_Bool isLVCode, const char* details,
+                           const char* location, const char* callStack,
+                           HAL_Bool printMsg) { return 0; });
+  }
+}
+
+void DriverStationSim::SetSendConsoleLine(bool shouldSend) {
+  if (shouldSend) {
+    HALSIM_SetSendConsoleLine(nullptr);
+  } else {
+    HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
+  }
+}
+
+int64_t DriverStationSim::GetJoystickOutputs(int stick) {
+  int64_t outputs = 0;
+  int32_t leftRumble;
+  int32_t rightRumble;
+  HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+  return outputs;
+}
+
+int DriverStationSim::GetJoystickRumble(int stick, int rumbleNum) {
+  int64_t outputs;
+  int32_t leftRumble = 0;
+  int32_t rightRumble = 0;
+  HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+  return rumbleNum == 0 ? leftRumble : rightRumble;
+}
+
+void DriverStationSim::SetJoystickButton(int stick, int button, bool state) {
+  HALSIM_SetJoystickButton(stick, button, state);
+}
+
+void DriverStationSim::SetJoystickAxis(int stick, int axis, double value) {
+  HALSIM_SetJoystickAxis(stick, axis, value);
+}
+
+void DriverStationSim::SetJoystickPOV(int stick, int pov, int value) {
+  HALSIM_SetJoystickPOV(stick, pov, value);
+}
+
+void DriverStationSim::SetJoystickButtons(int stick, uint32_t buttons) {
+  HALSIM_SetJoystickButtonsValue(stick, buttons);
+}
+
+void DriverStationSim::SetJoystickAxisCount(int stick, int count) {
+  HALSIM_SetJoystickAxisCount(stick, count);
+}
+
+void DriverStationSim::SetJoystickPOVCount(int stick, int count) {
+  HALSIM_SetJoystickPOVCount(stick, count);
+}
+
+void DriverStationSim::SetJoystickButtonCount(int stick, int count) {
+  HALSIM_SetJoystickButtonCount(stick, count);
+}
+
+void DriverStationSim::SetJoystickIsXbox(int stick, bool isXbox) {
+  HALSIM_SetJoystickIsXbox(stick, isXbox);
+}
+
+void DriverStationSim::SetJoystickType(int stick, int type) {
+  HALSIM_SetJoystickType(stick, type);
+}
+
+void DriverStationSim::SetJoystickName(int stick, const char* name) {
+  HALSIM_SetJoystickName(stick, name);
+}
+
+void DriverStationSim::SetJoystickAxisType(int stick, int axis, int type) {
+  HALSIM_SetJoystickAxisType(stick, axis, type);
+}
+
+void DriverStationSim::SetGameSpecificMessage(const char* message) {
+  HALSIM_SetGameSpecificMessage(message);
+}
+
+void DriverStationSim::SetEventName(const char* name) {
+  HALSIM_SetEventName(name);
+}
+
+void DriverStationSim::SetMatchType(DriverStation::MatchType type) {
+  HALSIM_SetMatchType(static_cast<HAL_MatchType>(static_cast<int>(type)));
+}
+
+void DriverStationSim::SetMatchNumber(int matchNumber) {
+  HALSIM_SetMatchNumber(matchNumber);
+}
+
+void DriverStationSim::SetReplayNumber(int replayNumber) {
+  HALSIM_SetReplayNumber(replayNumber);
+}
+
+void DriverStationSim::ResetData() { HALSIM_ResetDriverStationData(); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
new file mode 100644
index 0000000..b12f00a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/DutyCycleEncoderSim.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/DutyCycleEncoder.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << "DutyCycleEncoder" << '[' << encoder.GetFPGAIndex() << ']';
+  frc::sim::SimDeviceSim deviceSim{fullname.c_str()};
+  m_simPosition = deviceSim.GetDouble("Position");
+  m_simDistancePerRotation = deviceSim.GetDouble("DistancePerRotation");
+}
+
+void DutyCycleEncoderSim::Set(units::turn_t turns) {
+  m_simPosition.Set(turns.to<double>());
+}
+
+void DutyCycleEncoderSim::SetDistance(double distance) {
+  m_simPosition.Set(distance / m_simDistancePerRotation.Get());
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
new file mode 100644
index 0000000..7ec19fd
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/DutyCycleSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/DutyCycleData.h>
+
+#include "frc/DutyCycle.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DutyCycleSim::DutyCycleSim(const DutyCycle& dutyCycle)
+    : m_index{dutyCycle.GetFPGAIndex()} {}
+
+DutyCycleSim DutyCycleSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindDutyCycleForChannel(channel);
+  if (index < 0) throw std::out_of_range("no duty cycle found for channel");
+  return DutyCycleSim{index};
+}
+
+DutyCycleSim DutyCycleSim::CreateForIndex(int index) {
+  return DutyCycleSim{index};
+}
+
+std::unique_ptr<CallbackStore> DutyCycleSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDutyCycleInitializedCallback);
+  store->SetUid(HALSIM_RegisterDutyCycleInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DutyCycleSim::GetInitialized() const {
+  return HALSIM_GetDutyCycleInitialized(m_index);
+}
+
+void DutyCycleSim::SetInitialized(bool initialized) {
+  HALSIM_SetDutyCycleInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> DutyCycleSim::RegisterFrequencyCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDutyCycleFrequencyCallback);
+  store->SetUid(HALSIM_RegisterDutyCycleFrequencyCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int DutyCycleSim::GetFrequency() const {
+  return HALSIM_GetDutyCycleFrequency(m_index);
+}
+
+void DutyCycleSim::SetFrequency(int count) {
+  HALSIM_SetDutyCycleFrequency(m_index, count);
+}
+
+std::unique_ptr<CallbackStore> DutyCycleSim::RegisterOutputCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDutyCycleOutputCallback);
+  store->SetUid(HALSIM_RegisterDutyCycleOutputCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DutyCycleSim::GetOutput() const {
+  return HALSIM_GetDutyCycleOutput(m_index);
+}
+
+void DutyCycleSim::SetOutput(double period) {
+  HALSIM_SetDutyCycleOutput(m_index, period);
+}
+
+void DutyCycleSim::ResetData() { HALSIM_ResetDutyCycleData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
new file mode 100644
index 0000000..89c09cb
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/ElevatorSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
+                         const DCMotor& gearbox, double gearing,
+                         units::meter_t drumRadius, units::meter_t minHeight,
+                         units::meter_t maxHeight,
+                         const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim(plant, measurementStdDevs),
+      m_gearbox(gearbox),
+      m_drumRadius(drumRadius),
+      m_minHeight(minHeight),
+      m_maxHeight(maxHeight),
+      m_gearing(gearing) {}
+
+ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
+                         units::kilogram_t carriageMass,
+                         units::meter_t drumRadius, units::meter_t minHeight,
+                         units::meter_t maxHeight,
+                         const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
+                                                     drumRadius, gearing),
+                      measurementStdDevs),
+      m_gearbox(gearbox),
+      m_drumRadius(drumRadius),
+      m_minHeight(minHeight),
+      m_maxHeight(maxHeight),
+      m_gearing(gearing) {}
+
+bool ElevatorSim::HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) < m_minHeight.to<double>();
+}
+
+bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) > m_maxHeight.to<double>();
+}
+
+units::meter_t ElevatorSim::GetPosition() const {
+  return units::meter_t{m_x(0)};
+}
+
+units::meters_per_second_t ElevatorSim::GetVelocity() const {
+  return units::meters_per_second_t{m_x(1)};
+}
+
+units::ampere_t ElevatorSim::GetCurrentDraw() const {
+  // I = V / R - omega / (Kv * R)
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output.
+
+  // v = r w, so w = v / r
+  units::meters_per_second_t velocity{m_x(1)};
+  units::radians_per_second_t motorVelocity =
+      velocity / m_drumRadius * m_gearing * 1_rad;
+
+  // Perform calculation and return.
+  return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+}
+
+Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
+    const Eigen::Matrix<double, 2, 1>& currentXhat,
+    const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
+  auto updatedXhat = RungeKutta(
+      [&](const Eigen::Matrix<double, 2, 1>& x,
+          const Eigen::Matrix<double, 1, 1>& u_)
+          -> Eigen::Matrix<double, 2, 1> {
+        return m_plant.A() * x + m_plant.B() * u_ + MakeMatrix<2, 1>(0.0, -9.8);
+      },
+      currentXhat, u, dt);
+  // Check for collision after updating x-hat.
+  if (HasHitLowerLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_minHeight.to<double>(), 0.0);
+  }
+  if (HasHitUpperLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
+  }
+  return updatedXhat;
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
new file mode 100644
index 0000000..5bc5d52
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
@@ -0,0 +1,189 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/EncoderSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/EncoderData.h>
+
+#include "frc/Encoder.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+EncoderSim::EncoderSim(const Encoder& encoder)
+    : m_index{encoder.GetFPGAIndex()} {}
+
+EncoderSim EncoderSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindEncoderForChannel(channel);
+  if (index < 0) throw std::out_of_range("no encoder found for channel");
+  return EncoderSim{index};
+}
+
+EncoderSim EncoderSim::CreateForIndex(int index) { return EncoderSim{index}; }
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderInitializedCallback);
+  store->SetUid(HALSIM_RegisterEncoderInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetInitialized() const {
+  return HALSIM_GetEncoderInitialized(m_index);
+}
+
+void EncoderSim::SetInitialized(bool initialized) {
+  HALSIM_SetEncoderInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterCountCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderCountCallback);
+  store->SetUid(HALSIM_RegisterEncoderCountCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int EncoderSim::GetCount() const { return HALSIM_GetEncoderCount(m_index); }
+
+void EncoderSim::SetCount(int count) { HALSIM_SetEncoderCount(m_index, count); }
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterPeriodCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderPeriodCallback);
+  store->SetUid(HALSIM_RegisterEncoderPeriodCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double EncoderSim::GetPeriod() const {
+  return HALSIM_GetEncoderPeriod(m_index);
+}
+
+void EncoderSim::SetPeriod(double period) {
+  HALSIM_SetEncoderPeriod(m_index, period);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterResetCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderResetCallback);
+  store->SetUid(HALSIM_RegisterEncoderResetCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetReset() const { return HALSIM_GetEncoderReset(m_index); }
+
+void EncoderSim::SetReset(bool reset) {
+  HALSIM_SetEncoderReset(m_index, reset);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterMaxPeriodCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderMaxPeriodCallback);
+  store->SetUid(HALSIM_RegisterEncoderMaxPeriodCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double EncoderSim::GetMaxPeriod() const {
+  return HALSIM_GetEncoderMaxPeriod(m_index);
+}
+
+void EncoderSim::SetMaxPeriod(double maxPeriod) {
+  HALSIM_SetEncoderMaxPeriod(m_index, maxPeriod);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterDirectionCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderDirectionCallback);
+  store->SetUid(HALSIM_RegisterEncoderDirectionCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetDirection() const {
+  return HALSIM_GetEncoderDirection(m_index);
+}
+
+void EncoderSim::SetDirection(bool direction) {
+  HALSIM_SetEncoderDirection(m_index, direction);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterReverseDirectionCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderReverseDirectionCallback);
+  store->SetUid(HALSIM_RegisterEncoderReverseDirectionCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetReverseDirection() const {
+  return HALSIM_GetEncoderReverseDirection(m_index);
+}
+
+void EncoderSim::SetReverseDirection(bool reverseDirection) {
+  HALSIM_SetEncoderReverseDirection(m_index, reverseDirection);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterSamplesToAverageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderSamplesToAverageCallback);
+  store->SetUid(HALSIM_RegisterEncoderSamplesToAverageCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int EncoderSim::GetSamplesToAverage() const {
+  return HALSIM_GetEncoderSamplesToAverage(m_index);
+}
+
+void EncoderSim::SetSamplesToAverage(int samplesToAverage) {
+  HALSIM_SetEncoderSamplesToAverage(m_index, samplesToAverage);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterDistancePerPulseCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderDistancePerPulseCallback);
+  store->SetUid(HALSIM_RegisterEncoderDistancePerPulseCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double EncoderSim::GetDistancePerPulse() const {
+  return HALSIM_GetEncoderDistancePerPulse(m_index);
+}
+
+void EncoderSim::SetDistancePerPulse(double distancePerPulse) {
+  HALSIM_SetEncoderDistancePerPulse(m_index, distancePerPulse);
+}
+
+void EncoderSim::ResetData() { HALSIM_ResetEncoderData(m_index); }
+
+void EncoderSim::SetDistance(double distance) {
+  HALSIM_SetEncoderDistance(m_index, distance);
+}
+
+double EncoderSim::GetDistance() { return HALSIM_GetEncoderDistance(m_index); }
+
+void EncoderSim::SetRate(double rate) { HALSIM_SetEncoderRate(m_index, rate); }
+
+double EncoderSim::GetRate() { return HALSIM_GetEncoderRate(m_index); }

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

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

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

diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
new file mode 100644
index 0000000..65fc3c8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/FlywheelSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant,
+                         const DCMotor& gearbox, double gearing,
+                         const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim<1, 1, 1>(plant, measurementStdDevs),
+      m_gearbox(gearbox),
+      m_gearing(gearing) {}
+
+FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing,
+                         units::kilogram_square_meter_t moi,
+                         const std::array<double, 1>& measurementStdDevs)
+    : FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing),
+                  gearbox, gearing, measurementStdDevs) {}
+
+units::radians_per_second_t FlywheelSim::GetAngularVelocity() const {
+  return units::radians_per_second_t{GetOutput(0)};
+}
+
+units::ampere_t FlywheelSim::GetCurrentDraw() const {
+  // I = V / R - omega / (Kv * R)
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output.
+  return m_gearbox.Current(GetAngularVelocity() * m_gearing,
+                           units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp b/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp
new file mode 100644
index 0000000..8df265a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/GenericHIDSim.h"
+
+#include "frc/GenericHID.h"
+#include "frc/simulation/DriverStationSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+GenericHIDSim::GenericHIDSim(const GenericHID& joystick)
+    : m_port{joystick.GetPort()} {}
+
+GenericHIDSim::GenericHIDSim(int port) : m_port{port} {}
+
+void GenericHIDSim::NotifyNewData() { DriverStationSim::NotifyNewData(); }
+
+void GenericHIDSim::SetRawButton(int button, bool value) {
+  DriverStationSim::SetJoystickButton(m_port, button, value);
+}
+
+void GenericHIDSim::SetRawAxis(int axis, double value) {
+  DriverStationSim::SetJoystickAxis(m_port, axis, value);
+}
+
+void GenericHIDSim::SetPOV(int pov, int value) {
+  DriverStationSim::SetJoystickPOV(m_port, pov, value);
+}
+
+void GenericHIDSim::SetPOV(int value) { SetPOV(0, value); }
+
+void GenericHIDSim::SetAxisCount(int count) {
+  DriverStationSim::SetJoystickAxisCount(m_port, count);
+}
+
+void GenericHIDSim::SetPOVCount(int count) {
+  DriverStationSim::SetJoystickPOVCount(m_port, count);
+}
+
+void GenericHIDSim::SetButtonCount(int count) {
+  DriverStationSim::SetJoystickButtonCount(m_port, count);
+}
+
+void GenericHIDSim::SetType(GenericHID::HIDType type) {
+  DriverStationSim::SetJoystickType(m_port, type);
+}
+
+void GenericHIDSim::SetName(const char* name) {
+  DriverStationSim::SetJoystickName(m_port, name);
+}
+
+void GenericHIDSim::SetAxisType(int axis, int type) {
+  DriverStationSim::SetJoystickAxisType(m_port, axis, type);
+}
+
+bool GenericHIDSim::GetOutput(int outputNumber) {
+  int64_t outputs = GetOutputs();
+  return (outputs & (static_cast<int64_t>(1) << (outputNumber - 1))) != 0;
+}
+
+int64_t GenericHIDSim::GetOutputs() {
+  return DriverStationSim::GetJoystickOutputs(m_port);
+}
+
+double GenericHIDSim::GetRumble(GenericHID::RumbleType type) {
+  int value = DriverStationSim::GetJoystickRumble(
+      m_port, type == GenericHID::kLeftRumble ? 0 : 1);
+  return value / 65535.0;
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp b/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp
new file mode 100644
index 0000000..55f6089
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/JoystickSim.h"
+
+#include "frc/Joystick.h"
+#include "frc/simulation/GenericHIDSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+JoystickSim::JoystickSim(const Joystick& joystick)
+    : GenericHIDSim{joystick}, m_joystick{&joystick} {
+  // default to a reasonable joystick configuration
+  SetAxisCount(5);
+  SetButtonCount(12);
+  SetPOVCount(1);
+}
+
+JoystickSim::JoystickSim(int port) : GenericHIDSim{port} {
+  // default to a reasonable joystick configuration
+  SetAxisCount(5);
+  SetButtonCount(12);
+  SetPOVCount(1);
+}
+
+void JoystickSim::SetX(double value) {
+  SetRawAxis(
+      m_joystick ? m_joystick->GetXChannel() : Joystick::kDefaultXChannel,
+      value);
+}
+
+void JoystickSim::SetY(double value) {
+  SetRawAxis(
+      m_joystick ? m_joystick->GetYChannel() : Joystick::kDefaultYChannel,
+      value);
+}
+
+void JoystickSim::SetZ(double value) {
+  SetRawAxis(
+      m_joystick ? m_joystick->GetZChannel() : Joystick::kDefaultZChannel,
+      value);
+}
+
+void JoystickSim::SetTwist(double value) {
+  SetRawAxis(m_joystick ? m_joystick->GetTwistChannel()
+                        : Joystick::kDefaultTwistChannel,
+             value);
+}
+
+void JoystickSim::SetThrottle(double value) {
+  SetRawAxis(m_joystick ? m_joystick->GetThrottleChannel()
+                        : Joystick::kDefaultThrottleChannel,
+             value);
+}
+
+void JoystickSim::SetTrigger(bool state) { SetRawButton(1, state); }
+
+void JoystickSim::SetTop(bool state) { SetRawButton(2, state); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp b/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp
new file mode 100644
index 0000000..43178b1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/Mechanism2D.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/Twine.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+Mechanism2D::Mechanism2D() : m_device{"Mechanism2D"} {}
+
+void Mechanism2D::SetLigamentAngle(const wpi::Twine& ligamentPath,
+                                   float angle) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/angle").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, angle);
+    }
+    createdItems[fullPath].Set(angle);
+  }
+}
+
+void Mechanism2D::SetLigamentLength(const wpi::Twine& ligamentPath,
+                                    float length) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/length").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, length);
+    }
+    createdItems[fullPath].Set(length);
+  }
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp b/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp
new file mode 100644
index 0000000..5d0566a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/PCMSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/PCMData.h>
+
+#include "frc/Compressor.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PCMSim::PCMSim() : m_index{SensorUtil::GetDefaultSolenoidModule()} {}
+
+PCMSim::PCMSim(int module) : m_index{module} {}
+
+PCMSim::PCMSim(const Compressor& compressor)
+    : m_index{compressor.GetModule()} {}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterSolenoidInitializedCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback,
+      &HALSIM_CancelPCMSolenoidInitializedCallback);
+  store->SetUid(HALSIM_RegisterPCMSolenoidInitializedCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetSolenoidInitialized(int channel) const {
+  return HALSIM_GetPCMSolenoidInitialized(m_index, channel);
+}
+
+void PCMSim::SetSolenoidInitialized(int channel, bool solenoidInitialized) {
+  HALSIM_SetPCMSolenoidInitialized(m_index, channel, solenoidInitialized);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterSolenoidOutputCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback, &HALSIM_CancelPCMSolenoidOutputCallback);
+  store->SetUid(HALSIM_RegisterPCMSolenoidOutputCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetSolenoidOutput(int channel) const {
+  return HALSIM_GetPCMSolenoidOutput(m_index, channel);
+}
+
+void PCMSim::SetSolenoidOutput(int channel, bool solenoidOutput) {
+  HALSIM_SetPCMSolenoidOutput(m_index, channel, solenoidOutput);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterCompressorInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMCompressorInitializedCallback);
+  store->SetUid(HALSIM_RegisterPCMCompressorInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetCompressorInitialized() const {
+  return HALSIM_GetPCMCompressorInitialized(m_index);
+}
+
+void PCMSim::SetCompressorInitialized(bool compressorInitialized) {
+  HALSIM_SetPCMCompressorInitialized(m_index, compressorInitialized);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterCompressorOnCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMCompressorOnCallback);
+  store->SetUid(HALSIM_RegisterPCMCompressorOnCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetCompressorOn() const {
+  return HALSIM_GetPCMCompressorOn(m_index);
+}
+
+void PCMSim::SetCompressorOn(bool compressorOn) {
+  HALSIM_SetPCMCompressorOn(m_index, compressorOn);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterClosedLoopEnabledCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMClosedLoopEnabledCallback);
+  store->SetUid(HALSIM_RegisterPCMClosedLoopEnabledCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetClosedLoopEnabled() const {
+  return HALSIM_GetPCMClosedLoopEnabled(m_index);
+}
+
+void PCMSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
+  HALSIM_SetPCMClosedLoopEnabled(m_index, closedLoopEnabled);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterPressureSwitchCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMPressureSwitchCallback);
+  store->SetUid(HALSIM_RegisterPCMPressureSwitchCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetPressureSwitch() const {
+  return HALSIM_GetPCMPressureSwitch(m_index);
+}
+
+void PCMSim::SetPressureSwitch(bool pressureSwitch) {
+  HALSIM_SetPCMPressureSwitch(m_index, pressureSwitch);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterCompressorCurrentCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMCompressorCurrentCallback);
+  store->SetUid(HALSIM_RegisterPCMCompressorCurrentCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PCMSim::GetCompressorCurrent() const {
+  return HALSIM_GetPCMCompressorCurrent(m_index);
+}
+
+void PCMSim::SetCompressorCurrent(double compressorCurrent) {
+  HALSIM_SetPCMCompressorCurrent(m_index, compressorCurrent);
+}
+
+uint8_t PCMSim::GetAllSolenoidOutputs() const {
+  uint8_t ret = 0;
+  HALSIM_GetPCMAllSolenoids(m_index, &ret);
+  return ret;
+}
+
+void PCMSim::SetAllSolenoidOutputs(uint8_t outputs) {
+  HALSIM_SetPCMAllSolenoids(m_index, outputs);
+}
+
+void PCMSim::ResetData() { HALSIM_ResetPCMData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp b/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp
new file mode 100644
index 0000000..26d6a45
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/PDPSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/PDPData.h>
+
+#include "frc/PowerDistributionPanel.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PDPSim::PDPSim(int module) : m_index{module} {}
+
+PDPSim::PDPSim(const PowerDistributionPanel& pdp) : m_index{pdp.GetModule()} {}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPDPInitializedCallback);
+  store->SetUid(HALSIM_RegisterPDPInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PDPSim::GetInitialized() const {
+  return HALSIM_GetPDPInitialized(m_index);
+}
+
+void PDPSim::SetInitialized(bool initialized) {
+  HALSIM_SetPDPInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterTemperatureCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPDPTemperatureCallback);
+  store->SetUid(HALSIM_RegisterPDPTemperatureCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PDPSim::GetTemperature() const {
+  return HALSIM_GetPDPTemperature(m_index);
+}
+
+void PDPSim::SetTemperature(double temperature) {
+  HALSIM_SetPDPTemperature(m_index, temperature);
+}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPDPVoltageCallback);
+  store->SetUid(HALSIM_RegisterPDPVoltageCallback(m_index, &CallbackStoreThunk,
+                                                  store.get(), initialNotify));
+  return store;
+}
+
+double PDPSim::GetVoltage() const { return HALSIM_GetPDPVoltage(m_index); }
+
+void PDPSim::SetVoltage(double voltage) {
+  HALSIM_SetPDPVoltage(m_index, voltage);
+}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterCurrentCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback, &HALSIM_CancelPDPCurrentCallback);
+  store->SetUid(HALSIM_RegisterPDPCurrentCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PDPSim::GetCurrent(int channel) const {
+  return HALSIM_GetPDPCurrent(m_index, channel);
+}
+
+void PDPSim::SetCurrent(int channel, double current) {
+  HALSIM_SetPDPCurrent(m_index, channel, current);
+}
+
+void PDPSim::GetAllCurrents(double* currents) const {
+  HALSIM_GetPDPAllCurrents(m_index, currents);
+}
+
+void PDPSim::SetAllCurrents(const double* currents) {
+  HALSIM_SetPDPAllCurrents(m_index, currents);
+}
+
+void PDPSim::ResetData() { HALSIM_ResetPDPData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
new file mode 100644
index 0000000..2fae8fb
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/PWMSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/PWMData.h>
+
+#include "frc/PWM.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PWMSim::PWMSim(const PWM& pwm) : m_index{pwm.GetChannel()} {}
+
+PWMSim::PWMSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMInitializedCallback);
+  store->SetUid(HALSIM_RegisterPWMInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PWMSim::GetInitialized() const {
+  return HALSIM_GetPWMInitialized(m_index);
+}
+
+void PWMSim::SetInitialized(bool initialized) {
+  HALSIM_SetPWMInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterRawValueCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMRawValueCallback);
+  store->SetUid(HALSIM_RegisterPWMRawValueCallback(m_index, &CallbackStoreThunk,
+                                                   store.get(), initialNotify));
+  return store;
+}
+
+int PWMSim::GetRawValue() const { return HALSIM_GetPWMRawValue(m_index); }
+
+void PWMSim::SetRawValue(int rawValue) {
+  HALSIM_SetPWMRawValue(m_index, rawValue);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterSpeedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(m_index, -1, callback,
+                                               &HALSIM_CancelPWMSpeedCallback);
+  store->SetUid(HALSIM_RegisterPWMSpeedCallback(m_index, &CallbackStoreThunk,
+                                                store.get(), initialNotify));
+  return store;
+}
+
+double PWMSim::GetSpeed() const { return HALSIM_GetPWMSpeed(m_index); }
+
+void PWMSim::SetSpeed(double speed) { HALSIM_SetPWMSpeed(m_index, speed); }
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterPositionCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMPositionCallback);
+  store->SetUid(HALSIM_RegisterPWMPositionCallback(m_index, &CallbackStoreThunk,
+                                                   store.get(), initialNotify));
+  return store;
+}
+
+double PWMSim::GetPosition() const { return HALSIM_GetPWMPosition(m_index); }
+
+void PWMSim::SetPosition(double position) {
+  HALSIM_SetPWMPosition(m_index, position);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterPeriodScaleCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMPeriodScaleCallback);
+  store->SetUid(HALSIM_RegisterPWMPeriodScaleCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int PWMSim::GetPeriodScale() const { return HALSIM_GetPWMPeriodScale(m_index); }
+
+void PWMSim::SetPeriodScale(int periodScale) {
+  HALSIM_SetPWMPeriodScale(m_index, periodScale);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterZeroLatchCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMZeroLatchCallback);
+  store->SetUid(HALSIM_RegisterPWMZeroLatchCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PWMSim::GetZeroLatch() const { return HALSIM_GetPWMZeroLatch(m_index); }
+
+void PWMSim::SetZeroLatch(bool zeroLatch) {
+  HALSIM_SetPWMZeroLatch(m_index, zeroLatch);
+}
+
+void PWMSim::ResetData() { HALSIM_ResetPWMData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp b/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
new file mode 100644
index 0000000..7f7aa65
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/RelaySim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/RelayData.h>
+
+#include "frc/Relay.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+RelaySim::RelaySim(const Relay& relay) : m_index{relay.GetChannel()} {}
+
+RelaySim::RelaySim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterInitializedForwardCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayInitializedForwardCallback);
+  store->SetUid(HALSIM_RegisterRelayInitializedForwardCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetInitializedForward() const {
+  return HALSIM_GetRelayInitializedForward(m_index);
+}
+
+void RelaySim::SetInitializedForward(bool initializedForward) {
+  HALSIM_SetRelayInitializedForward(m_index, initializedForward);
+}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterInitializedReverseCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayInitializedReverseCallback);
+  store->SetUid(HALSIM_RegisterRelayInitializedReverseCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetInitializedReverse() const {
+  return HALSIM_GetRelayInitializedReverse(m_index);
+}
+
+void RelaySim::SetInitializedReverse(bool initializedReverse) {
+  HALSIM_SetRelayInitializedReverse(m_index, initializedReverse);
+}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterForwardCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayForwardCallback);
+  store->SetUid(HALSIM_RegisterRelayForwardCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetForward() const { return HALSIM_GetRelayForward(m_index); }
+
+void RelaySim::SetForward(bool forward) {
+  HALSIM_SetRelayForward(m_index, forward);
+}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterReverseCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayReverseCallback);
+  store->SetUid(HALSIM_RegisterRelayReverseCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetReverse() const { return HALSIM_GetRelayReverse(m_index); }
+
+void RelaySim::SetReverse(bool reverse) {
+  HALSIM_SetRelayReverse(m_index, reverse);
+}
+
+void RelaySim::ResetData() { HALSIM_ResetRelayData(m_index); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
new file mode 100644
index 0000000..0d1d104
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
@@ -0,0 +1,259 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/RoboRioSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/RoboRioData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterFPGAButtonCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioFPGAButtonCallback);
+  store->SetUid(HALSIM_RegisterRoboRioFPGAButtonCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetFPGAButton() { return HALSIM_GetRoboRioFPGAButton(); }
+
+void RoboRioSim::SetFPGAButton(bool fPGAButton) {
+  HALSIM_SetRoboRioFPGAButton(fPGAButton);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioVInVoltageCallback);
+  store->SetUid(HALSIM_RegisterRoboRioVInVoltageCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetVInVoltage() {
+  return units::volt_t(HALSIM_GetRoboRioVInVoltage());
+}
+
+void RoboRioSim::SetVInVoltage(units::volt_t vInVoltage) {
+  HALSIM_SetRoboRioVInVoltage(vInVoltage.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInCurrentCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioVInCurrentCallback);
+  store->SetUid(HALSIM_RegisterRoboRioVInCurrentCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetVInCurrent() {
+  return units::ampere_t(HALSIM_GetRoboRioVInCurrent());
+}
+
+void RoboRioSim::SetVInCurrent(units::ampere_t vInCurrent) {
+  HALSIM_SetRoboRioVInCurrent(vInCurrent.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserVoltage6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserVoltage6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetUserVoltage6V() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage6V());
+}
+
+void RoboRioSim::SetUserVoltage6V(units::volt_t userVoltage6V) {
+  HALSIM_SetRoboRioUserVoltage6V(userVoltage6V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserCurrent6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserCurrent6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetUserCurrent6V() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent6V());
+}
+
+void RoboRioSim::SetUserCurrent6V(units::ampere_t userCurrent6V) {
+  HALSIM_SetRoboRioUserCurrent6V(userCurrent6V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserActive6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserActive6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetUserActive6V() { return HALSIM_GetRoboRioUserActive6V(); }
+
+void RoboRioSim::SetUserActive6V(bool userActive6V) {
+  HALSIM_SetRoboRioUserActive6V(userActive6V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserVoltage5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserVoltage5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetUserVoltage5V() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage5V());
+}
+
+void RoboRioSim::SetUserVoltage5V(units::volt_t userVoltage5V) {
+  HALSIM_SetRoboRioUserVoltage5V(userVoltage5V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserCurrent5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserCurrent5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetUserCurrent5V() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent5V());
+}
+
+void RoboRioSim::SetUserCurrent5V(units::ampere_t userCurrent5V) {
+  HALSIM_SetRoboRioUserCurrent5V(userCurrent5V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserActive5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserActive5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetUserActive5V() { return HALSIM_GetRoboRioUserActive5V(); }
+
+void RoboRioSim::SetUserActive5V(bool userActive5V) {
+  HALSIM_SetRoboRioUserActive5V(userActive5V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserVoltage3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserVoltage3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetUserVoltage3V3() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage3V3());
+}
+
+void RoboRioSim::SetUserVoltage3V3(units::volt_t userVoltage3V3) {
+  HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserCurrent3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserCurrent3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetUserCurrent3V3() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent3V3());
+}
+
+void RoboRioSim::SetUserCurrent3V3(units::ampere_t userCurrent3V3) {
+  HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserActive3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserActive3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetUserActive3V3() { return HALSIM_GetRoboRioUserActive3V3(); }
+
+void RoboRioSim::SetUserActive3V3(bool userActive3V3) {
+  HALSIM_SetRoboRioUserActive3V3(userActive3V3);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserFaults6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserFaults6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserFaults6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int RoboRioSim::GetUserFaults6V() { return HALSIM_GetRoboRioUserFaults6V(); }
+
+void RoboRioSim::SetUserFaults6V(int userFaults6V) {
+  HALSIM_SetRoboRioUserFaults6V(userFaults6V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserFaults5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserFaults5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserFaults5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int RoboRioSim::GetUserFaults5V() { return HALSIM_GetRoboRioUserFaults5V(); }
+
+void RoboRioSim::SetUserFaults5V(int userFaults5V) {
+  HALSIM_SetRoboRioUserFaults5V(userFaults5V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserFaults3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserFaults3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserFaults3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int RoboRioSim::GetUserFaults3V3() { return HALSIM_GetRoboRioUserFaults3V3(); }
+
+void RoboRioSim::SetUserFaults3V3(int userFaults3V3) {
+  HALSIM_SetRoboRioUserFaults3V3(userFaults3V3);
+}
+
+void ResetData() { HALSIM_ResetRoboRioData(); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
new file mode 100644
index 0000000..b71f99d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/SPIAccelerometerSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/SPIAccelerometerData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+SPIAccelerometerSim::SPIAccelerometerSim(int index) { m_index = index; }
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterActiveCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerActiveCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerActiveCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool SPIAccelerometerSim::GetActive() const {
+  return HALSIM_GetSPIAccelerometerActive(m_index);
+}
+
+void SPIAccelerometerSim::SetActive(bool active) {
+  HALSIM_SetSPIAccelerometerActive(m_index, active);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterRangeCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerRangeCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerRangeCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int SPIAccelerometerSim::GetRange() const {
+  return HALSIM_GetSPIAccelerometerRange(m_index);
+}
+
+void SPIAccelerometerSim::SetRange(int range) {
+  HALSIM_SetSPIAccelerometerRange(m_index, range);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterXCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerXCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerXCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double SPIAccelerometerSim::GetX() const {
+  return HALSIM_GetSPIAccelerometerX(m_index);
+}
+
+void SPIAccelerometerSim::SetX(double x) {
+  HALSIM_SetSPIAccelerometerX(m_index, x);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterYCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerYCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerYCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double SPIAccelerometerSim::GetY() const {
+  return HALSIM_GetSPIAccelerometerY(m_index);
+}
+
+void SPIAccelerometerSim::SetY(double y) {
+  HALSIM_SetSPIAccelerometerY(m_index, y);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterZCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerZCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerZCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double SPIAccelerometerSim::GetZ() const {
+  return HALSIM_GetSPIAccelerometerZ(m_index);
+}
+
+void SPIAccelerometerSim::SetZ(double z) {
+  HALSIM_SetSPIAccelerometerZ(m_index, z);
+}
+
+void SPIAccelerometerSim::ResetData() {
+  HALSIM_ResetSPIAccelerometerData(m_index);
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp b/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp
new file mode 100644
index 0000000..3d52cdf
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/SimDeviceSim.h"
+
+#include <string>
+#include <vector>
+
+#include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+SimDeviceSim::SimDeviceSim(const char* name)
+    : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
+
+hal::SimValue SimDeviceSim::GetValue(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+hal::SimDouble SimDeviceSim::GetDouble(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+hal::SimEnum SimDeviceSim::GetEnum(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+hal::SimBoolean SimDeviceSim::GetBoolean(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+std::vector<std::string> SimDeviceSim::GetEnumOptions(hal::SimEnum val) {
+  int32_t numOptions;
+  const char** options = HALSIM_GetSimValueEnumOptions(val, &numOptions);
+  std::vector<std::string> rv;
+  rv.reserve(numOptions);
+  for (int32_t i = 0; i < numOptions; ++i) rv.emplace_back(options[i]);
+  return rv;
+}
+
+void SimDeviceSim::ResetData() { HALSIM_ResetSimDeviceData(); }

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

diff --git a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
new file mode 100644
index 0000000..28c434c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/SimHooks.h"
+
+#include <hal/simulation/MockHooks.h>
+
+namespace frc {
+namespace sim {
+
+void SetRuntimeType(HAL_RuntimeType type) { HALSIM_SetRuntimeType(type); }
+
+void WaitForProgramStart() { HALSIM_WaitForProgramStart(); }
+
+void SetProgramStarted() { HALSIM_SetProgramStarted(); }
+
+bool GetProgramStarted() { return HALSIM_GetProgramStarted(); }
+
+void RestartTiming() { HALSIM_RestartTiming(); }
+
+void PauseTiming() { HALSIM_PauseTiming(); }
+
+void ResumeTiming() { HALSIM_ResumeTiming(); }
+
+bool IsTimingPaused() { return HALSIM_IsTimingPaused(); }
+
+void StepTiming(units::second_t delta) {
+  HALSIM_StepTiming(static_cast<uint64_t>(delta.to<double>() * 1e6));
+}
+
+void StepTimingAsync(units::second_t delta) {
+  HALSIM_StepTimingAsync(static_cast<uint64_t>(delta.to<double>() * 1e6));
+}
+
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
new file mode 100644
index 0000000..b33a19e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/SingleJointedArmSim.h"
+
+#include <cmath>
+
+#include <units/voltage.h>
+#include <wpi/MathExtras.h>
+
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+SingleJointedArmSim::SingleJointedArmSim(
+    const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
+    units::meter_t armLength, units::radian_t minAngle,
+    units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
+    const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
+      m_r(armLength),
+      m_minAngle(minAngle),
+      m_maxAngle(maxAngle),
+      m_mass(mass),
+      m_gearbox(gearbox),
+      m_gearing(gearing),
+      m_simulateGravity(simulateGravity) {}
+
+SingleJointedArmSim::SingleJointedArmSim(
+    const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi,
+    units::meter_t armLength, units::radian_t minAngle,
+    units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
+    const std::array<double, 1>& measurementStdDevs)
+    : SingleJointedArmSim(
+          LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
+          gearbox, gearing, armLength, minAngle, maxAngle, mass,
+          simulateGravity, measurementStdDevs) {}
+
+bool SingleJointedArmSim::HasHitLowerLimit(
+    const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) < m_minAngle.to<double>();
+}
+
+bool SingleJointedArmSim::HasHitUpperLimit(
+    const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) > m_maxAngle.to<double>();
+}
+
+units::radian_t SingleJointedArmSim::GetAngle() const {
+  return units::radian_t{m_x(0)};
+}
+
+units::radians_per_second_t SingleJointedArmSim::GetVelocity() const {
+  return units::radians_per_second_t{m_x(1)};
+}
+
+units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output
+  units::radians_per_second_t motorVelocity{m_x(1) * m_gearing};
+  return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+}
+
+Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
+    const Eigen::Matrix<double, 2, 1>& currentXhat,
+    const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
+  // Horizontal case:
+  // Torque = F * r = I * alpha
+  // alpha = F * r / I
+  // Since F = mg,
+  // alpha = m * g * r / I
+  // Finally, multiply RHS by cos(theta) to account for the arm angle
+  // This acceleration is added to the linear system dynamics x-dot = Ax + Bu
+  // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
+  // std::cos(theta)]]
+
+  auto updatedXhat = RungeKutta(
+      [&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
+        Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;
+
+        if (m_simulateGravity) {
+          xdot += MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 /
+                                         (m_mass * m_r * m_r) * std::cos(x(0)))
+                                            .template to<double>());
+        }
+        return xdot;
+      },
+      currentXhat, u, dt);
+
+  // Check for collisions.
+  if (HasHitLowerLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_minAngle.to<double>(), 0.0);
+  } else if (HasHitUpperLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_maxAngle.to<double>(), 0.0);
+  }
+  return updatedXhat;
+}

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

diff --git a/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp b/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp
new file mode 100644
index 0000000..e3bbdce
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/XboxControllerSim.h"
+
+#include "frc/XboxController.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+XboxControllerSim::XboxControllerSim(const XboxController& joystick)
+    : GenericHIDSim{joystick} {
+  SetAxisCount(6);
+  SetButtonCount(10);
+}
+
+XboxControllerSim::XboxControllerSim(int port) : GenericHIDSim{port} {
+  SetAxisCount(6);
+  SetButtonCount(10);
+}
+
+void XboxControllerSim::SetX(GenericHID::JoystickHand hand, double value) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kLeftX), value);
+  } else {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kRightX), value);
+  }
+}
+
+void XboxControllerSim::SetY(GenericHID::JoystickHand hand, double value) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kLeftY), value);
+  } else {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kRightY), value);
+  }
+}
+
+void XboxControllerSim::SetTriggerAxis(GenericHID::JoystickHand hand,
+                                       double value) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kLeftTrigger), value);
+  } else {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kRightTrigger), value);
+  }
+}
+
+void XboxControllerSim::SetBumper(GenericHID::JoystickHand hand, bool state) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawButton(static_cast<int>(XboxController::Button::kBumperLeft), state);
+  } else {
+    SetRawButton(static_cast<int>(XboxController::Button::kBumperRight), state);
+  }
+}
+
+void XboxControllerSim::SetStickButton(GenericHID::JoystickHand hand,
+                                       bool state) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawButton(static_cast<int>(XboxController::Button::kStickLeft), state);
+  } else {
+    SetRawButton(static_cast<int>(XboxController::Button::kStickRight), state);
+  }
+}
+
+void XboxControllerSim::SetAButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kA), state);
+}
+
+void XboxControllerSim::SetBButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kB), state);
+}
+
+void XboxControllerSim::SetXButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kX), state);
+}
+
+void XboxControllerSim::SetYButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kY), state);
+}
+
+void XboxControllerSim::SetBackButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kBack), state);
+}
+
+void XboxControllerSim::SetStartButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kStart), state);
+}

wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp

diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
index d075deb..135f4be 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,7 +32,9 @@
   for (auto& property : m_properties) {
     if (property.update) property.update(property.entry, time);
   }
-  if (m_updateTable) m_updateTable();
+  for (auto& updateTable : m_updateTables) {
+    updateTable();
+  }
 }
 
 void SendableBuilderImpl::StartListeners() {
@@ -71,7 +73,7 @@
 }
 
 void SendableBuilderImpl::SetUpdateTable(std::function<void()> func) {
-  m_updateTable = func;
+  m_updateTables.emplace_back(std::move(func));
 }
 
 nt::NetworkTableEntry SendableBuilderImpl::GetEntry(const wpi::Twine& key) {

wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp

diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index 9d57ad6..4480313 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -163,7 +163,9 @@
 void SendableRegistry::Move(Sendable* to, Sendable* from) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(from);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   UID compUid = it->getSecond();
   m_impl->componentMap.erase(it);
   m_impl->componentMap[to] = compUid;
@@ -188,14 +190,18 @@
 std::string SendableRegistry::GetName(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return std::string{};
   return m_impl->components[it->getSecond() - 1]->name;
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->name = name.str();
 }
 
@@ -203,7 +209,9 @@
                                int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
 }
 
@@ -211,7 +219,9 @@
                                int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
                                                    channel);
 }
@@ -220,7 +230,9 @@
                                const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   comp.name = name.str();
   comp.subsystem = subsystem.str();
@@ -229,7 +241,9 @@
 std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return std::string{};
   return m_impl->components[it->getSecond() - 1]->subsystem;
 }
 
@@ -237,7 +251,9 @@
                                     const wpi::Twine& subsystem) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
 }
 
@@ -251,7 +267,9 @@
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return nullptr;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   std::shared_ptr<void> rv;
   if (static_cast<size_t>(handle) < comp.data.size())
@@ -267,7 +285,9 @@
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return nullptr;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   if (static_cast<size_t>(handle) >= comp.data.size()) return nullptr;
   return comp.data[handle];
@@ -276,14 +296,18 @@
 void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->liveWindow = true;
 }
 
 void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->liveWindow = false;
 }
 
@@ -298,13 +322,17 @@
 Sendable* SendableRegistry::GetSendable(UID uid) {
   if (uid == 0) return nullptr;
   std::scoped_lock lock(m_impl->mutex);
+  if ((uid - 1) >= m_impl->components.size() || !m_impl->components[uid - 1])
+    return nullptr;
   return m_impl->components[uid - 1]->sendable;
 }
 
 void SendableRegistry::Publish(UID sendableUid,
                                std::shared_ptr<NetworkTable> table) {
   std::scoped_lock lock(m_impl->mutex);
-  if (sendableUid == 0) return;
+  if (sendableUid == 0 || (sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   auto& comp = *m_impl->components[sendableUid - 1];
   comp.builder = SendableBuilderImpl{};  // clear any current builder
   comp.builder.SetTable(table);
@@ -316,6 +344,9 @@
 void SendableRegistry::Update(UID sendableUid) {
   if (sendableUid == 0) return;
   std::scoped_lock lock(m_impl->mutex);
+  if ((sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   m_impl->components[sendableUid - 1]->builder.UpdateTable();
 }
 
@@ -327,7 +358,7 @@
   wpi::SmallVector<Impl::Component*, 128> components;
   for (auto&& comp : m_impl->components) components.emplace_back(comp.get());
   for (auto comp : components) {
-    if (comp->sendable && comp->liveWindow) {
+    if (comp && comp->sendable && comp->liveWindow) {
       if (static_cast<size_t>(dataHandle) >= comp->data.size())
         comp->data.resize(dataHandle + 1);
       CallbackData cbdata{comp->sendable,         comp->name,

wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp

diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
deleted file mode 100644
index 3991fec..0000000
--- a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/spline/CubicHermiteSpline.h"
-
-using namespace frc;
-
-CubicHermiteSpline::CubicHermiteSpline(
-    std::array<double, 2> xInitialControlVector,
-    std::array<double, 2> xFinalControlVector,
-    std::array<double, 2> yInitialControlVector,
-    std::array<double, 2> yFinalControlVector) {
-  const auto hermite = MakeHermiteBasis();
-  const auto x =
-      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-  const auto y =
-      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-  // Populate first two rows with coefficients.
-  m_coefficients.template block<1, 4>(0, 0) = hermite * x;
-  m_coefficients.template block<1, 4>(1, 0) = hermite * y;
-
-  // Populate Row 2 and Row 3 with the derivatives of the equations above.
-  // Then populate row 4 and 5 with the second derivatives.
-  for (int i = 0; i < 4; i++) {
-    // Here, we are multiplying by (3 - i) to manually take the derivative. The
-    // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
-    // coefficient of the derivative, we can use the power rule and multiply
-    // the existing coefficient by its power.
-    m_coefficients.template block<2, 1>(2, i) =
-        m_coefficients.template block<2, 1>(0, i) * (3 - i);
-  }
-
-  for (int i = 0; i < 3; i++) {
-    // Here, we are multiplying by (2 - i) to manually take the derivative. The
-    // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
-    // coefficient of the derivative, we can use the power rule and multiply
-    // the existing coefficient by its power.
-    m_coefficients.template block<2, 1>(4, i) =
-        m_coefficients.template block<2, 1>(2, i) * (2 - i);
-  }
-}

wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp

diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
deleted file mode 100644
index bb8ad3c..0000000
--- a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/spline/QuinticHermiteSpline.h"
-
-using namespace frc;
-
-QuinticHermiteSpline::QuinticHermiteSpline(
-    std::array<double, 3> xInitialControlVector,
-    std::array<double, 3> xFinalControlVector,
-    std::array<double, 3> yInitialControlVector,
-    std::array<double, 3> yFinalControlVector) {
-  const auto hermite = MakeHermiteBasis();
-  const auto x =
-      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-  const auto y =
-      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-  // Populate first two rows with coefficients.
-  m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
-  m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
-
-  // Populate Row 2 and Row 3 with the derivatives of the equations above.
-  // Then populate row 4 and 5 with the second derivatives.
-  for (int i = 0; i < 6; i++) {
-    // Here, we are multiplying by (5 - i) to manually take the derivative. The
-    // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
-    // coefficient of the derivative, we can use the power rule and multiply
-    // the existing coefficient by its power.
-    m_coefficients.template block<2, 1>(2, i) =
-        m_coefficients.template block<2, 1>(0, i) * (5 - i);
-  }
-  for (int i = 0; i < 5; i++) {
-    // Here, we are multiplying by (4 - i) to manually take the derivative. The
-    // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
-    // coefficient of the derivative, we can use the power rule and multiply
-    // the existing coefficient by its power.
-    m_coefficients.template block<2, 1>(4, i) =
-        m_coefficients.template block<2, 1>(2, i) * (4 - i);
-  }
-}

wpilibc/src/main/native/cpp/spline/SplineHelper.cpp

diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
deleted file mode 100644
index cbfc8c1..0000000
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ /dev/null
@@ -1,208 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/spline/SplineHelper.h"
-
-#include <cstddef>
-
-using namespace frc;
-
-std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
-    const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
-    const Spline<3>::ControlVector& end) {
-  std::vector<CubicHermiteSpline> splines;
-
-  std::array<double, 2> xInitial = start.x;
-  std::array<double, 2> yInitial = start.y;
-  std::array<double, 2> xFinal = end.x;
-  std::array<double, 2> yFinal = end.y;
-
-  if (waypoints.size() > 1) {
-    waypoints.emplace(waypoints.begin(),
-                      Translation2d{units::meter_t(xInitial[0]),
-                                    units::meter_t(yInitial[0])});
-    waypoints.emplace_back(
-        Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
-
-    // Populate tridiagonal system for clamped cubic
-    /* See:
-    https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
-    /undervisningsmateriale/chap7alecture.pdf
-    */
-
-    // Above-diagonal of tridiagonal matrix, zero-padded
-    std::vector<double> a;
-    // Diagonal of tridiagonal matrix
-    std::vector<double> b(waypoints.size() - 2, 4.0);
-    // Below-diagonal of tridiagonal matrix, zero-padded
-    std::vector<double> c;
-    // rhs vectors
-    std::vector<double> dx, dy;
-    // solution vectors
-    std::vector<double> fx(waypoints.size() - 2, 0.0),
-        fy(waypoints.size() - 2, 0.0);
-
-    // populate above-diagonal and below-diagonal vectors
-    a.emplace_back(0);
-    for (size_t i = 0; i < waypoints.size() - 3; ++i) {
-      a.emplace_back(1);
-      c.emplace_back(1);
-    }
-    c.emplace_back(0);
-
-    // populate rhs vectors
-    dx.emplace_back(
-        3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
-        xInitial[1]);
-    dy.emplace_back(
-        3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
-        yInitial[1]);
-    if (waypoints.size() > 4) {
-      for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
-        dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
-                             waypoints[i - 1].X().to<double>()));
-        dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
-                             waypoints[i - 1].Y().to<double>()));
-      }
-    }
-    dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
-                         waypoints[waypoints.size() - 3].X().to<double>()) -
-                    xFinal[1]);
-    dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
-                         waypoints[waypoints.size() - 3].Y().to<double>()) -
-                    yFinal[1]);
-
-    // Compute solution to tridiagonal system
-    ThomasAlgorithm(a, b, c, dx, &fx);
-    ThomasAlgorithm(a, b, c, dy, &fy);
-
-    fx.emplace(fx.begin(), xInitial[1]);
-    fx.emplace_back(xFinal[1]);
-    fy.emplace(fy.begin(), yInitial[1]);
-    fy.emplace_back(yFinal[1]);
-
-    for (size_t i = 0; i < fx.size() - 1; ++i) {
-      // Create the spline.
-      const CubicHermiteSpline spline{
-          {waypoints[i].X().to<double>(), fx[i]},
-          {waypoints[i + 1].X().to<double>(), fx[i + 1]},
-          {waypoints[i].Y().to<double>(), fy[i]},
-          {waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
-
-      splines.push_back(spline);
-    }
-  } else if (waypoints.size() == 1) {
-    const double xDeriv =
-        (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
-    const double yDeriv =
-        (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
-
-    std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
-                                            xDeriv};
-    std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
-                                            yDeriv};
-
-    splines.emplace_back(xInitial, midXControlVector, yInitial,
-                         midYControlVector);
-    splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
-
-  } else {
-    // Create the spline.
-    const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
-    splines.push_back(spline);
-  }
-
-  return splines;
-}
-
-std::vector<QuinticHermiteSpline>
-SplineHelper::QuinticSplinesFromControlVectors(
-    const std::vector<Spline<5>::ControlVector>& controlVectors) {
-  std::vector<QuinticHermiteSpline> splines;
-  for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
-    auto& xInitial = controlVectors[i].x;
-    auto& yInitial = controlVectors[i].y;
-    auto& xFinal = controlVectors[i + 1].x;
-    auto& yFinal = controlVectors[i + 1].y;
-    splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
-  }
-  return splines;
-}
-
-std::array<Spline<3>::ControlVector, 2>
-SplineHelper::CubicControlVectorsFromWaypoints(
-    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-    const Pose2d& end) {
-  double scalar;
-  if (interiorWaypoints.empty()) {
-    scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
-  } else {
-    scalar =
-        1.2 *
-        start.Translation().Distance(interiorWaypoints.front()).to<double>();
-  }
-  const auto initialCV = CubicControlVector(scalar, start);
-  if (!interiorWaypoints.empty()) {
-    scalar =
-        1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
-  }
-  const auto finalCV = CubicControlVector(scalar, end);
-  return {initialCV, finalCV};
-}
-
-std::vector<Spline<5>::ControlVector>
-SplineHelper::QuinticControlVectorsFromWaypoints(
-    const std::vector<Pose2d>& waypoints) {
-  std::vector<Spline<5>::ControlVector> vectors;
-  for (size_t i = 0; i < waypoints.size() - 1; ++i) {
-    auto& p0 = waypoints[i];
-    auto& p1 = waypoints[i + 1];
-
-    // This just makes the splines look better.
-    const auto scalar =
-        1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
-
-    vectors.push_back(QuinticControlVector(scalar, p0));
-    vectors.push_back(QuinticControlVector(scalar, p1));
-  }
-  return vectors;
-}
-
-void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
-                                   const std::vector<double>& b,
-                                   const std::vector<double>& c,
-                                   const std::vector<double>& d,
-                                   std::vector<double>* solutionVector) {
-  auto& f = *solutionVector;
-  size_t N = d.size();
-
-  // Create the temporary vectors
-  // Note that this is inefficient as it is possible to call
-  // this function many times. A better implementation would
-  // pass these temporary matrices by non-const reference to
-  // save excess allocation and deallocation
-  std::vector<double> c_star(N, 0.0);
-  std::vector<double> d_star(N, 0.0);
-
-  // This updates the coefficients in the first row
-  // Note that we should be checking for division by zero here
-  c_star[0] = c[0] / b[0];
-  d_star[0] = d[0] / b[0];
-
-  // Create the c_star and d_star coefficients in the forward sweep
-  for (size_t i = 1; i < N; ++i) {
-    double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
-    c_star[i] = c[i] * m;
-    d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
-  }
-
-  f[N - 1] = d_star[N - 1];
-  // This is the reverse sweep, used to update the solution vector f
-  for (int i = N - 2; i >= 0; i--) {
-    f[i] = d_star[i] - c_star[i] * f[i + 1];
-  }
-}

wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp

diff --git a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
deleted file mode 100644
index f3c42c6..0000000
--- a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/spline/SplineParameterizer.h"
-
-constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
-constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
-constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;

wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
deleted file mode 100644
index 84d4f37..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/Trajectory.h"
-
-#include <wpi/json.h>
-
-using namespace frc;
-
-bool Trajectory::State::operator==(const Trajectory::State& other) const {
-  return t == other.t && velocity == other.velocity &&
-         acceleration == other.acceleration && pose == other.pose &&
-         curvature == other.curvature;
-}
-
-bool Trajectory::State::operator!=(const Trajectory::State& other) const {
-  return !operator==(other);
-}
-
-Trajectory::State Trajectory::State::Interpolate(State endValue,
-                                                 double i) const {
-  // Find the new [t] value.
-  const auto newT = Lerp(t, endValue.t, i);
-
-  // Find the delta time between the current state and the interpolated state.
-  const auto deltaT = newT - t;
-
-  // If delta time is negative, flip the order of interpolation.
-  if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
-
-  // Check whether the robot is reversing at this stage.
-  const auto reversing =
-      velocity < 0_mps ||
-      (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
-
-  // Calculate the new velocity.
-  // v = v_0 + at
-  const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
-
-  // Calculate the change in position.
-  // delta_s = v_0 t + 0.5 at^2
-  const units::meter_t newS =
-      (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
-      (reversing ? -1.0 : 1.0);
-
-  // Return the new state. To find the new position for the new state, we need
-  // to interpolate between the two endpoint poses. The fraction for
-  // interpolation is the change in position (delta s) divided by the total
-  // distance between the two endpoints.
-  const double interpolationFrac =
-      newS / endValue.pose.Translation().Distance(pose.Translation());
-
-  return {newT, newV, acceleration,
-          Lerp(pose, endValue.pose, interpolationFrac),
-          Lerp(curvature, endValue.curvature, interpolationFrac)};
-}
-
-Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
-  m_totalTime = states.back().t;
-}
-
-Trajectory::State Trajectory::Sample(units::second_t t) const {
-  if (t <= m_states.front().t) return m_states.front();
-  if (t >= m_totalTime) return m_states.back();
-
-  // To get the element that we want, we will use a binary search algorithm
-  // instead of iterating over a for-loop. A binary search is O(std::log(n))
-  // whereas searching using a loop is O(n).
-
-  // This starts at 1 because we use the previous state later on for
-  // interpolation.
-  int low = 1;
-  int high = m_states.size() - 1;
-
-  while (low != high) {
-    int mid = (low + high) / 2;
-    if (m_states[mid].t < t) {
-      // This index and everything under it are less than the requested
-      // timestamp. Therefore, we can discard them.
-      low = mid + 1;
-    } else {
-      // t is at least as large as the element at this index. This means that
-      // anything after it cannot be what we are looking for.
-      high = mid;
-    }
-  }
-
-  // High and Low should be the same.
-
-  // The sample's timestamp is now greater than or equal to the requested
-  // timestamp. If it is greater, we need to interpolate between the
-  // previous state and the current state to get the exact state that we
-  // want.
-  const auto sample = m_states[low];
-  const auto prevSample = m_states[low - 1];
-
-  // If the difference in states is negligible, then we are spot on!
-  if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) {
-    return sample;
-  }
-  // Interpolate between the two states for the state that we want.
-  return prevSample.Interpolate(sample,
-                                (t - prevSample.t) / (sample.t - prevSample.t));
-}
-
-Trajectory Trajectory::TransformBy(const Transform2d& transform) {
-  auto& firstState = m_states[0];
-  auto& firstPose = firstState.pose;
-
-  // Calculate the transformed first pose.
-  auto newFirstPose = firstPose + transform;
-  auto newStates = m_states;
-  newStates[0].pose = newFirstPose;
-
-  for (unsigned int i = 1; i < newStates.size(); i++) {
-    auto& state = newStates[i];
-    // We are transforming relative to the coordinate frame of the new initial
-    // pose.
-    state.pose = newFirstPose + (state.pose - firstPose);
-  }
-
-  return Trajectory(newStates);
-}
-
-Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
-  auto newStates = m_states;
-  for (auto& state : newStates) {
-    state.pose = state.pose.RelativeTo(pose);
-  }
-  return Trajectory(newStates);
-}
-
-void frc::to_json(wpi::json& json, const Trajectory::State& state) {
-  json = wpi::json{{"time", state.t.to<double>()},
-                   {"velocity", state.velocity.to<double>()},
-                   {"acceleration", state.acceleration.to<double>()},
-                   {"pose", state.pose},
-                   {"curvature", state.curvature.to<double>()}};
-}
-
-void frc::from_json(const wpi::json& json, Trajectory::State& state) {
-  state.pose = json.at("pose").get<Pose2d>();
-  state.t = units::second_t{json.at("time").get<double>()};
-  state.velocity =
-      units::meters_per_second_t{json.at("velocity").get<double>()};
-  state.acceleration =
-      units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
-  state.curvature = frc::curvature_t{json.at("curvature").get<double>()};
-}

wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
deleted file mode 100644
index 3c95472..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/TrajectoryGenerator.h"
-
-#include <utility>
-
-#include "frc/DriverStation.h"
-#include "frc/spline/SplineHelper.h"
-#include "frc/spline/SplineParameterizer.h"
-#include "frc/trajectory/TrajectoryParameterizer.h"
-
-using namespace frc;
-
-const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
-    std::vector<Trajectory::State>{Trajectory::State()});
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    Spline<3>::ControlVector initial,
-    const std::vector<Translation2d>& interiorWaypoints,
-    Spline<3>::ControlVector end, const TrajectoryConfig& config) {
-  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
-
-  // Make theta normal for trajectory generation if path is reversed.
-  // Flip the headings.
-  if (config.IsReversed()) {
-    initial.x[1] *= -1;
-    initial.y[1] *= -1;
-    end.x[1] *= -1;
-    end.y[1] *= -1;
-  }
-
-  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
-  try {
-    points =
-        SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
-            initial, interiorWaypoints, end));
-  } catch (SplineParameterizer::MalformedSplineException& e) {
-    DriverStation::ReportError(e.what());
-    return kDoNothingTrajectory;
-  }
-
-  // After trajectory generation, flip theta back so it's relative to the
-  // field. Also fix curvature.
-  if (config.IsReversed()) {
-    for (auto& point : points) {
-      point = {point.first + flip, -point.second};
-    }
-  }
-
-  return TrajectoryParameterizer::TimeParameterizeTrajectory(
-      points, config.Constraints(), config.StartVelocity(),
-      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
-      config.IsReversed());
-}
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-    const Pose2d& end, const TrajectoryConfig& config) {
-  auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
-      start, interiorWaypoints, end);
-  return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
-}
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    std::vector<Spline<5>::ControlVector> controlVectors,
-    const TrajectoryConfig& config) {
-  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
-  // Make theta normal for trajectory generation if path is reversed.
-  if (config.IsReversed()) {
-    for (auto& vector : controlVectors) {
-      // Flip the headings.
-      vector.x[1] *= -1;
-      vector.y[1] *= -1;
-    }
-  }
-
-  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
-  try {
-    points = SplinePointsFromSplines(
-        SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
-  } catch (SplineParameterizer::MalformedSplineException& e) {
-    DriverStation::ReportError(e.what());
-    return kDoNothingTrajectory;
-  }
-
-  // After trajectory generation, flip theta back so it's relative to the
-  // field. Also fix curvature.
-  if (config.IsReversed()) {
-    for (auto& point : points) {
-      point = {point.first + flip, -point.second};
-    }
-  }
-
-  return TrajectoryParameterizer::TimeParameterizeTrajectory(
-      points, config.Constraints(), config.StartVelocity(),
-      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
-      config.IsReversed());
-}
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
-  return GenerateTrajectory(
-      SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
-}

wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
deleted file mode 100644
index 131ae8a..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ /dev/null
@@ -1,224 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include "frc/trajectory/TrajectoryParameterizer.h"
-
-using namespace frc;
-
-Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
-    const std::vector<PoseWithCurvature>& points,
-    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-    units::meters_per_second_t startVelocity,
-    units::meters_per_second_t endVelocity,
-    units::meters_per_second_t maxVelocity,
-    units::meters_per_second_squared_t maxAcceleration, bool reversed) {
-  std::vector<ConstrainedState> constrainedStates(points.size());
-
-  ConstrainedState predecessor{points.front(), 0_m, startVelocity,
-                               -maxAcceleration, maxAcceleration};
-
-  constrainedStates[0] = predecessor;
-
-  // Forward pass
-  for (unsigned int i = 0; i < points.size(); i++) {
-    auto& constrainedState = constrainedStates[i];
-    constrainedState.pose = points[i];
-
-    // Begin constraining based on predecessor
-    units::meter_t ds = constrainedState.pose.first.Translation().Distance(
-        predecessor.pose.first.Translation());
-    constrainedState.distance = ds + predecessor.distance;
-
-    // We may need to iterate to find the maximum end velocity and common
-    // acceleration, since acceleration limits may be a function of velocity.
-    while (true) {
-      // Enforce global max velocity and max reachable velocity by global
-      // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
-
-      constrainedState.maxVelocity = units::math::min(
-          maxVelocity,
-          units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
-                            predecessor.maxAcceleration * ds * 2.0));
-
-      constrainedState.minAcceleration = -maxAcceleration;
-      constrainedState.maxAcceleration = maxAcceleration;
-
-      // At this point, the constrained state is fully constructed apart from
-      // all the custom-defined user constraints.
-      for (const auto& constraint : constraints) {
-        constrainedState.maxVelocity = units::math::min(
-            constrainedState.maxVelocity,
-            constraint->MaxVelocity(constrainedState.pose.first,
-                                    constrainedState.pose.second,
-                                    constrainedState.maxVelocity));
-      }
-
-      // Now enforce all acceleration limits.
-      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
-
-      if (ds.to<double>() < kEpsilon) break;
-
-      // If the actual acceleration for this state is higher than the max
-      // acceleration that we applied, then we need to reduce the max
-      // acceleration of the predecessor and try again.
-      units::meters_per_second_squared_t actualAcceleration =
-          (constrainedState.maxVelocity * constrainedState.maxVelocity -
-           predecessor.maxVelocity * predecessor.maxVelocity) /
-          (ds * 2.0);
-
-      // If we violate the max acceleration constraint, let's modify the
-      // predecessor.
-      if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) {
-        predecessor.maxAcceleration = constrainedState.maxAcceleration;
-      } else {
-        // Constrain the predecessor's max acceleration to the current
-        // acceleration.
-        if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) {
-          predecessor.maxAcceleration = actualAcceleration;
-        }
-        // If the actual acceleration is less than the predecessor's min
-        // acceleration, it will be repaired in the backward pass.
-        break;
-      }
-    }
-    predecessor = constrainedState;
-  }
-
-  ConstrainedState successor{points.back(), constrainedStates.back().distance,
-                             endVelocity, -maxAcceleration, maxAcceleration};
-
-  // Backward pass
-  for (int i = points.size() - 1; i >= 0; i--) {
-    auto& constrainedState = constrainedStates[i];
-    units::meter_t ds =
-        constrainedState.distance - successor.distance;  // negative
-
-    while (true) {
-      // Enforce max velocity limit (reverse)
-      // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
-      units::meters_per_second_t newMaxVelocity =
-          units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
-                            successor.minAcceleration * ds * 2.0);
-
-      // No more limits to impose! This state can be finalized.
-      if (newMaxVelocity >= constrainedState.maxVelocity) break;
-
-      constrainedState.maxVelocity = newMaxVelocity;
-
-      // Check all acceleration constraints with the new max velocity.
-      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
-
-      if (ds.to<double>() > -kEpsilon) break;
-
-      // If the actual acceleration for this state is lower than the min
-      // acceleration, then we need to lower the min acceleration of the
-      // successor and try again.
-      units::meters_per_second_squared_t actualAcceleration =
-          (constrainedState.maxVelocity * constrainedState.maxVelocity -
-           successor.maxVelocity * successor.maxVelocity) /
-          (ds * 2.0);
-      if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) {
-        successor.minAcceleration = constrainedState.minAcceleration;
-      } else {
-        successor.minAcceleration = actualAcceleration;
-        break;
-      }
-    }
-    successor = constrainedState;
-  }
-
-  // Now we can integrate the constrained states forward in time to obtain our
-  // trajectory states.
-
-  std::vector<Trajectory::State> states(points.size());
-  units::second_t t = 0_s;
-  units::meter_t s = 0_m;
-  units::meters_per_second_t v = 0_mps;
-
-  for (unsigned int i = 0; i < constrainedStates.size(); i++) {
-    auto state = constrainedStates[i];
-
-    // Calculate the change in position between the current state and the
-    // previous state.
-    units::meter_t ds = state.distance - s;
-
-    // Calculate the acceleration between the current state and the previous
-    // state.
-    units::meters_per_second_squared_t accel =
-        (state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
-
-    // Calculate dt.
-    units::second_t dt = 0_s;
-    if (i > 0) {
-      states.at(i - 1).acceleration = reversed ? -accel : accel;
-      if (units::math::abs(accel) > 1E-6_mps_sq) {
-        // v_f = v_0 + a * t
-        dt = (state.maxVelocity - v) / accel;
-      } else if (units::math::abs(v) > 1E-6_mps) {
-        // delta_x = v * t
-        dt = ds / v;
-      } else {
-        throw std::runtime_error(
-            "Something went wrong during trajectory generation.");
-      }
-    }
-
-    v = state.maxVelocity;
-    s = state.distance;
-
-    t += dt;
-
-    states[i] = {t, reversed ? -v : v, reversed ? -accel : accel,
-                 state.pose.first, state.pose.second};
-  }
-
-  return Trajectory(states);
-}
-
-void TrajectoryParameterizer::EnforceAccelerationLimits(
-    bool reverse,
-    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-    ConstrainedState* state) {
-  for (auto&& constraint : constraints) {
-    double factor = reverse ? -1.0 : 1.0;
-
-    auto minMaxAccel = constraint->MinMaxAcceleration(
-        state->pose.first, state->pose.second, state->maxVelocity * factor);
-
-    state->minAcceleration = units::math::max(
-        state->minAcceleration,
-        reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
-
-    state->maxAcceleration = units::math::min(
-        state->maxAcceleration,
-        reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
-  }
-}

wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
deleted file mode 100644
index f3cf30d..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/TrajectoryUtil.h"
-
-#include <system_error>
-
-#include <wpi/SmallString.h>
-#include <wpi/json.h>
-#include <wpi/raw_istream.h>
-#include <wpi/raw_ostream.h>
-
-using namespace frc;
-
-void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
-                                      const wpi::Twine& path) {
-  std::error_code error_code;
-
-  wpi::SmallString<128> buf;
-  wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
-  if (error_code) {
-    throw std::runtime_error(("Cannot open file: " + path).str());
-  }
-
-  wpi::json json = trajectory.States();
-  output << json;
-  output.flush();
-}
-
-Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
-  std::error_code error_code;
-
-  wpi::SmallString<128> buf;
-  wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
-  if (error_code) {
-    throw std::runtime_error(("Cannot open file: " + path).str());
-  }
-
-  wpi::json json;
-  input >> json;
-
-  return Trajectory{json.get<std::vector<Trajectory::State>>()};
-}
-
-std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
-  wpi::json json = trajectory.States();
-  return json.dump();
-}
-
-Trajectory TrajectoryUtil::DeserializeTrajectory(
-    const wpi::StringRef json_str) {
-  wpi::json json = wpi::json::parse(json_str);
-  return Trajectory{json.get<std::vector<Trajectory::State>>()};
-}

wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
deleted file mode 100644
index bf45c34..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
-
-using namespace frc;
-
-CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
-    units::meters_per_second_squared_t maxCentripetalAcceleration)
-    : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
-
-units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t velocity) {
-  // ac = v^2 / r
-  // k (curvature) = 1 / r
-
-  // therefore, ac = v^2 * k
-  // ac / k = v^2
-  // v = std::sqrt(ac / k)
-
-  // We have to multiply by 1_rad here to get the units to cancel out nicely.
-  // The units library defines a unit for radians although it is technically
-  // unitless.
-  return units::math::sqrt(m_maxCentripetalAcceleration /
-                           units::math::abs(curvature) * 1_rad);
-}
-
-TrajectoryConstraint::MinMax
-CentripetalAccelerationConstraint::MinMaxAcceleration(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t speed) {
-  // The acceleration of the robot has no impact on the centripetal acceleration
-  // of the robot.
-  return {};
-}

wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
deleted file mode 100644
index 8b88bf4..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-
-using namespace frc;
-
-DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
-    DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
-    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
-
-units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t velocity) {
-  auto wheelSpeeds =
-      m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
-  wheelSpeeds.Normalize(m_maxSpeed);
-
-  return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
-}
-
-TrajectoryConstraint::MinMax
-DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t speed) {
-  return {};
-}

wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
deleted file mode 100644
index 3d6b61c..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
-
-#include <algorithm>
-#include <limits>
-
-#include <wpi/MathExtras.h>
-
-using namespace frc;
-
-DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
-    SimpleMotorFeedforward<units::meter> feedforward,
-    DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
-    : m_feedforward(feedforward),
-      m_kinematics(kinematics),
-      m_maxVoltage(maxVoltage) {}
-
-units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t velocity) {
-  return units::meters_per_second_t(std::numeric_limits<double>::max());
-}
-
-TrajectoryConstraint::MinMax
-DifferentialDriveVoltageConstraint::MinMaxAcceleration(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t speed) {
-  auto wheelSpeeds =
-      m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
-
-  auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
-  auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
-
-  // Calculate maximum/minimum possible accelerations from motor dynamics
-  // and max/min wheel speeds
-  auto maxWheelAcceleration =
-      m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
-  auto minWheelAcceleration =
-      m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
-
-  // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
-  // increased by half of the trackwidth T.  Inner wheel has radius decreased
-  // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
-  // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2)
-  // Inner wheel is similar.
-
-  // sgn(speed) term added to correctly account for which wheel is on
-  // outside of turn:
-  // If moving forward, max acceleration constraint corresponds to wheel on
-  // outside of turn
-  // If moving backward, max acceleration constraint corresponds to wheel on
-  // inside of turn
-  auto maxChassisAcceleration =
-      maxWheelAcceleration /
-      (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
-               wpi::sgn(speed) / (2_rad));
-  auto minChassisAcceleration =
-      minWheelAcceleration /
-      (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
-               wpi::sgn(speed) / (2_rad));
-
-  // Negate acceleration corresponding to wheel on inside of turn
-  // if center of turn is inside of wheelbase
-  if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
-    if (speed > 0_mps) {
-      minChassisAcceleration = -minChassisAcceleration;
-    } else {
-      maxChassisAcceleration = -maxChassisAcceleration;
-    }
-  }
-
-  return {minChassisAcceleration, maxChassisAcceleration};
-}

wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp

diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
deleted file mode 100644
index 2fd8151..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
-
-using namespace frc;
-
-MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
-    MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
-    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
-
-units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t velocity) {
-  auto xVelocity = velocity * pose.Rotation().Cos();
-  auto yVelocity = velocity * pose.Rotation().Sin();
-  auto wheelSpeeds =
-      m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
-  wheelSpeeds.Normalize(m_maxSpeed);
-
-  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
-}
-
-TrajectoryConstraint::MinMax
-MecanumDriveKinematicsConstraint::MinMaxAcceleration(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t speed) {
-  return {};
-}

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

diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index eaf4ddc..5f34e77 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,6 +17,7 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpimath/MathShared.h>
 
 #include "WPILibVersion.h"
 #include "frc/DriverStation.h"
@@ -68,6 +69,47 @@
     return std::make_pair(RobotBase::GetThreadId(), true);
   }
 };
+class WPILibMathShared : public wpi::math::MathShared {
+ public:
+  void ReportError(const wpi::Twine& error) override {
+    DriverStation::ReportError(error);
+  }
+
+  void ReportUsage(wpi::math::MathUsageId id, int count) override {
+    switch (id) {
+      case wpi::math::MathUsageId::kKinematics_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_MecanumDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kTrajectory_TrapezoidProfile:
+        HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, count);
+        break;
+      case wpi::math::MathUsageId::kFilter_Linear:
+        HAL_Report(HALUsageReporting::kResourceType_LinearFilter, count);
+        break;
+      case wpi::math::MathUsageId::kOdometry_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_MecanumDrive);
+        break;
+    }
+  }
+};
 }  // namespace
 
 static void SetupCameraServerShared() {
@@ -88,8 +130,6 @@
   if (symbol) {
     auto setCameraServerShared = (SetCameraServerSharedFP)symbol;
     setCameraServerShared(new WPILibCameraServerShared{});
-    wpi::outs() << "Set Camera Server Shared\n";
-    wpi::outs().flush();
   } else {
     wpi::outs() << "Camera Server Shared Symbol Missing\n";
     wpi::outs().flush();
@@ -103,14 +143,27 @@
 #endif
 }
 
+static void SetupMathShared() {
+  wpi::math::MathSharedStore::SetMathShared(
+      std::make_unique<WPILibMathShared>());
+}
+
 bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
 
 bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
 
 bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
 
+bool RobotBase::IsAutonomousEnabled() const {
+  return m_ds.IsAutonomousEnabled();
+}
+
 bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
 
+bool RobotBase::IsOperatorControlEnabled() const {
+  return m_ds.IsOperatorControlEnabled();
+}
+
 bool RobotBase::IsTest() const { return m_ds.IsTest(); }
 
 bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }
@@ -121,6 +174,7 @@
   m_threadId = std::this_thread::get_id();
 
   SetupCameraServerShared();
+  SetupMathShared();
 
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.SetNetworkIdentity("Robot");

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

diff --git a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
index ccdb75c..309fab3 100644
--- a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
+++ b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -27,6 +27,7 @@
  * determine the heading.
  *
  * This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ * Only one instance of an ADXRS Gyro is supported.
  */
 class ADXRS450_Gyro : public GyroBase {
  public:
@@ -50,21 +51,19 @@
   /**
    * Return the actual angle in degrees that the robot is currently facing.
    *
-   * The angle is based on the current accumulator value corrected by the
-   * oversampling rate, the gyro type and the A/D calibration values.
+   * The angle is based on integration of the returned rate from the gyro.
    * The angle is continuous, that is it will continue from 360->361 degrees.
    * This allows algorithms that wouldn't want to see a discontinuity in the
    * gyro output as it sweeps from 360 to 0 on the second time around.
    *
-   * @return the current heading of the robot in degrees. This heading is based
-   *         on integration of the returned rate from the gyro.
+   * @return the current heading of the robot in degrees.
    */
   double GetAngle() const override;
 
   /**
    * Return the rate of rotation of the gyro
    *
-   * The rate is based on the most recent reading of the gyro analog value
+   * The rate is based on the most recent reading of the gyro.
    *
    * @return the current rate in degrees per second
    */
@@ -93,8 +92,16 @@
    */
   void Calibrate() override;
 
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  int GetPort() const;
+
  private:
   SPI m_spi;
+  SPI::Port m_port;
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simAngle;

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

diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h
index 5534ae4..3bcbab5 100644
--- a/wpilibc/src/main/native/include/frc/AddressableLED.h
+++ b/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/AddressableLEDTypes.h>
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 
 #include "frc/ErrorBase.h"
@@ -22,6 +22,8 @@
 
 /**
  * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ *
+ * <p>Only 1 LED driver is currently supported by the roboRIO.
  */
 class AddressableLED : public ErrorBase {
  public:
@@ -83,7 +85,7 @@
   /**
    * Constructs a new driver for a specific port.
    *
-   * @param port the output port to use (Must be a PWM port)
+   * @param port the output port to use (Must be a PWM header)
    */
   explicit AddressableLED(int port);
 
@@ -95,6 +97,8 @@
    * <p>Calling this is an expensive call, so its best to call it once, then
    * just update data.
    *
+   * <p>The max length is 5460 LEDs.
+   *
    * @param length the strip length
    */
   void SetLength(int length);
@@ -147,7 +151,7 @@
   /**
    * Starts the output.
    *
-   * <p>The output writes continously.
+   * <p>The output writes continuously.
    */
   void Start();
 

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

diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
index 872db87..3dc22ea 100644
--- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h
+++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/SimDevice.h>
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/Counter.h"
@@ -108,6 +108,13 @@
    */
   double GetDistance() const;
 
+  /**
+   * Get the channel number.
+   *
+   * @return The channel number.
+   */
+  int GetChannel() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  private:

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

diff --git a/wpilibc/src/main/native/include/frc/AnalogGyro.h b/wpilibc/src/main/native/include/frc/AnalogGyro.h
index bcc67c9..b59df5b 100644
--- a/wpilibc/src/main/native/include/frc/AnalogGyro.h
+++ b/wpilibc/src/main/native/include/frc/AnalogGyro.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -186,6 +186,13 @@
 
   void Calibrate() override;
 
+  /**
+   * Gets the analog input for the gyro.
+   *
+   * @return AnalogInput
+   */
+  std::shared_ptr<AnalogInput> GetAnalogInput() const;
+
  protected:
   std::shared_ptr<AnalogInput> m_analog;
 

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

diff --git a/wpilibc/src/main/native/include/frc/AnalogOutput.h b/wpilibc/src/main/native/include/frc/AnalogOutput.h
index 1cecd70..026986b 100644
--- a/wpilibc/src/main/native/include/frc/AnalogOutput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogOutput.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -55,7 +55,7 @@
   /**
    * Get the channel of this AnalogOutput.
    */
-  int GetChannel();
+  int GetChannel() const;
 
   void InitSendable(SendableBuilder& builder) override;
 

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

diff --git a/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h b/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
index 446a920..ed943f7 100644
--- a/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
+++ b/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -42,11 +42,12 @@
    * This will calculate the result from the fullRange times the fraction of the
    * supply voltage, plus the offset.
    *
-   * @param channel   The channel number on the roboRIO to represent. 0-3 are
-   *                  on-board 4-7 are on the MXP port.
-   * @param fullRange The angular value (in desired units) representing the full
+   * @param channel   The Analog Input channel number on the roboRIO the
+   *                  potentiometer is plugged into. 0-3 are on-board and 4-7
+   *                  are on the MXP port.
+   * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
-   * @param offset    The angular value (in desired units) representing the
+   * @param offset    The value (in desired units) representing the
    *                  angular output at 0V.
    */
   explicit AnalogPotentiometer(int channel, double fullRange = 1.0,
@@ -66,9 +67,9 @@
    * supply voltage, plus the offset.
    *
    * @param channel   The existing Analog Input pointer
-   * @param fullRange The angular value (in desired units) representing the full
+   * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
-   * @param offset    The angular value (in desired units) representing the
+   * @param offset    The value (in desired units) representing the
    *                  angular output at 0V.
    */
   explicit AnalogPotentiometer(AnalogInput* input, double fullRange = 1.0,
@@ -88,9 +89,9 @@
    * supply voltage, plus the offset.
    *
    * @param channel   The existing Analog Input pointer
-   * @param fullRange The angular value (in desired units) representing the full
+   * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
-   * @param offset    The angular value (in desired units) representing the
+   * @param offset    The value (in desired units) representing the
    *                  angular output at 0V.
    */
   explicit AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
@@ -102,7 +103,7 @@
   AnalogPotentiometer& operator=(AnalogPotentiometer&&) = default;
 
   /**
-   * Get the current reading of the potentiomer.
+   * Get the current reading of the potentiometer.
    *
    * @return The current position of the potentiometer (in the units used for
    *         fullRange and offset).

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

diff --git a/wpilibc/src/main/native/include/frc/AnalogTrigger.h b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
index 7554bd9..d465652 100644
--- a/wpilibc/src/main/native/include/frc/AnalogTrigger.h
+++ b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -101,17 +101,6 @@
   void SetAveraged(bool useAveragedValue);
 
   /**
-   * Configure the analog trigger to use the duty cycle vs. raw values.
-   *
-   * If the value is true, then the duty cycle value is selected for the analog
-   * trigger, otherwise the immediate value is used.
-   *
-   * @param useDutyCycle If true, use the duty cycle value, otherwise use the
-   *                         instantaneous reading
-   */
-  void SetDutyCycle(bool useDutyCycle);
-
-  /**
    * Configure the analog trigger to use a filtered value.
    *
    * The analog trigger will operate with a 3 point average rejection filter.

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

diff --git a/wpilibc/src/main/native/include/frc/CAN.h b/wpilibc/src/main/native/include/frc/CAN.h
index ed861fb..612d533 100644
--- a/wpilibc/src/main/native/include/frc/CAN.h
+++ b/wpilibc/src/main/native/include/frc/CAN.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 #include <stdint.h>
 
 #include <hal/CANAPITypes.h>
-#include <wpi/ArrayRef.h>
 
 #include "frc/ErrorBase.h"
 
@@ -94,6 +93,38 @@
   void WriteRTRFrame(int length, int apiId);
 
   /**
+   * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+   *
+   * @param data The data to write (8 bytes max)
+   * @param length The data length to write
+   * @param apiId The API ID to write.
+   */
+  int WritePacketNoError(const uint8_t* data, int length, int apiId);
+
+  /**
+   * Write a repeating packet to the CAN device with a specific ID. This ID is
+   * 10 bits. The RoboRIO will automatically repeat the packet at the specified
+   * interval
+   *
+   * @param data The data to write (8 bytes max)
+   * @param length The data length to write
+   * @param apiId The API ID to write.
+   * @param repeatMs The period to repeat the packet at.
+   */
+  int WritePacketRepeatingNoError(const uint8_t* data, int length, int apiId,
+                                  int repeatMs);
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10
+   * bits. The length by spec must match what is returned by the responding
+   * device
+   *
+   * @param length The length to request (0 to 8)
+   * @param apiId The API ID to write.
+   */
+  int WriteRTRFrameNoError(int length, int apiId);
+
+  /**
    * Stop a repeating packet with a specific ID. This ID is 10 bits.
    *
    * @param apiId The API ID to stop repeating

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

diff --git a/wpilibc/src/main/native/include/frc/Compressor.h b/wpilibc/src/main/native/include/frc/Compressor.h
index a5e512c..d1a4dca 100644
--- a/wpilibc/src/main/native/include/frc/Compressor.h
+++ b/wpilibc/src/main/native/include/frc/Compressor.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -171,6 +171,13 @@
    */
   void ClearAllPCMStickyFaults();
 
+  /**
+   * Gets module number (CAN ID).
+   *
+   * @return Module number
+   */
+  int GetModule() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  protected:

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

diff --git a/wpilibc/src/main/native/include/frc/Controller.h b/wpilibc/src/main/native/include/frc/Controller.h
index 4124046..c6b25ab 100644
--- a/wpilibc/src/main/native/include/frc/Controller.h
+++ b/wpilibc/src/main/native/include/frc/Controller.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,10 +17,12 @@
  * Common interface for controllers. Controllers run control loops, the most
  * common are PID controllers and their variants, but this includes anything
  * that is controlling an actuator in a separate thread.
+ *
+ * @deprecated Only used by the deprecated PIDController
  */
 class Controller {
  public:
-  WPI_DEPRECATED("None of the 2020 FRC controllers use this.")
+  WPI_DEPRECATED("Only used by the deprecated PIDController")
   Controller() = default;
   virtual ~Controller() = default;
 

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

diff --git a/wpilibc/src/main/native/include/frc/DMASample.h b/wpilibc/src/main/native/include/frc/DMASample.h
index 4592930..c286c8d 100644
--- a/wpilibc/src/main/native/include/frc/DMASample.h
+++ b/wpilibc/src/main/native/include/frc/DMASample.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/AnalogInput.h>
 #include <hal/DMA.h>
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc/AnalogInput.h"
 #include "frc/Counter.h"

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

diff --git a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
index 6722976..302c52c 100644
--- a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
+++ b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -69,6 +69,15 @@
   virtual Value Get() const;
 
   /**
+   * Toggle the value of the solenoid.
+   *
+   * If the solenoid is set to forward, it'll be set to reverse. If the solenoid
+   * is set to reverse, it'll be set to forward. If the solenoid is set to off,
+   * nothing happens.
+   */
+  void Toggle();
+
+  /**
    * Check if the forward solenoid is blacklisted.
    *
    * If a solenoid is shorted, it is added to the blacklist and disabled until

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

diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h
index 4508701..a6c40d6 100644
--- a/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,11 +16,9 @@
 #include <hal/DriverStationTypes.h>
 #include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
-#include <wpi/deprecated.h>
 #include <wpi/mutex.h>
 
 #include "frc/ErrorBase.h"
-#include "frc/RobotState.h"
 
 namespace frc {
 
@@ -184,6 +182,17 @@
   int GetJoystickAxisType(int stick, int axis) const;
 
   /**
+   * Returns if a joystick is connected to the Driver Station.
+   *
+   * This makes a best effort guess by looking at the reported number of axis,
+   * buttons, and POVs attached.
+   *
+   * @param stick The joystick port number
+   * @return true if a joystick is connected
+   */
+  bool IsJoystickConnected(int stick) const;
+
+  /**
    * Check if the DS has enabled the robot.
    *
    * @return True if the robot is enabled and the DS is connected
@@ -212,6 +221,15 @@
   bool IsAutonomous() const;
 
   /**
+   * Check if the DS is commanding autonomous mode and if it has enabled the
+   * robot.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode and
+   * enabled.
+   */
+  bool IsAutonomousEnabled() const;
+
+  /**
    * Check if the DS is commanding teleop mode.
    *
    * @return True if the robot is being commanded to be in teleop mode
@@ -219,6 +237,14 @@
   bool IsOperatorControl() const;
 
   /**
+   * Check if the DS is commanding teleop mode and if it has enabled the robot.
+   *
+   * @return True if the robot is being commanded to be in teleop mode and
+   * enabled.
+   */
+  bool IsOperatorControlEnabled() const;
+
+  /**
    * Check if the DS is commanding test mode.
    *
    * @return True if the robot is being commanded to be in test mode
@@ -313,6 +339,9 @@
    *
    * This is a good way to delay processing until there is new driver station
    * data to act on.
+   *
+   * Checks if new control data has arrived since the last waitForData call
+   * on the current thread. If new data has not arrived, returns immediately.
    */
   void WaitForData();
 
@@ -320,6 +349,9 @@
    * Wait until a new packet comes from the driver station, or wait for a
    * timeout.
    *
+   * Checks if new control data has arrived since the last waitForData call
+   * on the current thread. If new data has not arrived, returns immediately.
+   *
    * If the timeout is less then or equal to 0, wait indefinitely.
    *
    * Timeout is in milliseconds
@@ -345,7 +377,7 @@
    * Warning: This is not an official time (so it cannot be used to dispute ref
    * calls or guarantee that a function will trigger before the match ends).
    *
-   * The Practice Match function of the DS approximates the behaviour seen on
+   * The Practice Match function of the DS approximates the behavior seen on
    * the field.
    *
    * @return Time remaining in current match period (auto or teleop)
@@ -399,6 +431,23 @@
    */
   void WakeupWaitForData();
 
+  /**
+   * Allows the user to specify whether they want joystick connection warnings
+   * to be printed to the console. This setting is ignored when the FMS is
+   * connected -- warnings will always be on in that scenario.
+   *
+   * @param silence Whether warning messages should be silenced.
+   */
+  void SilenceJoystickConnectionWarning(bool silence);
+
+  /**
+   * Returns whether joystick connection warnings are silenced. This will
+   * always return false when connected to the FMS.
+   *
+   * @return Whether joystick connection warnings are silenced.
+   */
+  bool IsJoystickConnectionWarningSilenced() const;
+
  protected:
   /**
    * Copy data from the DS task for the user.
@@ -446,10 +495,12 @@
   std::thread m_dsThread;
   std::atomic<bool> m_isRunning{false};
 
-  wpi::mutex m_waitForDataMutex;
+  mutable wpi::mutex m_waitForDataMutex;
   wpi::condition_variable m_waitForDataCond;
   int m_waitForDataCounter;
 
+  bool m_silenceJoystickWarning = false;
+
   // Robot state status variables
   bool m_userInDisabled = false;
   bool m_userInAutonomous = false;

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

diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
index 92864a8..17b038f 100644
--- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
+++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/SimDevice.h>
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/Counter.h"
@@ -154,14 +154,28 @@
    */
   double GetDistance() const;
 
+  /**
+   * Get the FPGA index for the DutyCycleEncoder.
+   *
+   * @return the FPGA index
+   */
+  int GetFPGAIndex() const;
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  int GetSourceChannel() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  private:
   void Init();
 
   std::shared_ptr<DutyCycle> m_dutyCycle;
-  AnalogTrigger m_analogTrigger;
-  Counter m_counter;
+  std::unique_ptr<AnalogTrigger> m_analogTrigger;
+  std::unique_ptr<Counter> m_counter;
   int m_frequencyThreshold = 100;
   double m_positionOffset = 0;
   double m_distancePerRotation = 1.0;
@@ -169,6 +183,7 @@
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simPosition;
+  hal::SimDouble m_simDistancePerRotation;
   hal::SimBoolean m_simIsConnected;
 };
 }  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/Encoder.h b/wpilibc/src/main/native/include/frc/Encoder.h
index 60552f6..76f9d1c 100644
--- a/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/wpilibc/src/main/native/include/frc/Encoder.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -192,6 +192,9 @@
    *                  the FPGA will report the device stopped. This is expressed
    *                  in seconds.
    */
+  WPI_DEPRECATED(
+      "Use SetMinRate() in favor of this method.  This takes unscaled periods "
+      "and SetMinRate() scales using value from SetDistancePerPulse().")
   void SetMaxPeriod(double maxPeriod) override;
 
   /**

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

diff --git a/wpilibc/src/main/native/include/frc/Error.h b/wpilibc/src/main/native/include/frc/Error.h
index 8eafd10..d63fb62 100644
--- a/wpilibc/src/main/native/include/frc/Error.h
+++ b/wpilibc/src/main/native/include/frc/Error.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,6 @@
 
 #pragma once
 
-#include <stdint.h>
-
 #include <string>
 
 #include <wpi/StringRef.h>
@@ -19,8 +17,6 @@
 #undef GetMessage
 #endif
 
-#include "frc/Base.h"
-
 namespace frc {
 
 class ErrorBase;

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

diff --git a/wpilibc/src/main/native/include/frc/ErrorBase.h b/wpilibc/src/main/native/include/frc/ErrorBase.h
index e9168aa..0ced9a2 100644
--- a/wpilibc/src/main/native/include/frc/ErrorBase.h
+++ b/wpilibc/src/main/native/include/frc/ErrorBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,9 +11,7 @@
 
 #include <wpi/StringRef.h>
 #include <wpi/Twine.h>
-#include <wpi/mutex.h>
 
-#include "frc/Base.h"
 #include "frc/Error.h"
 
 // Forward declared manually to avoid needing to pull in entire HAL header.

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

diff --git a/wpilibc/src/main/native/include/frc/Filesystem.h b/wpilibc/src/main/native/include/frc/Filesystem.h
index b7ef3f1..f196f7a 100644
--- a/wpilibc/src/main/native/include/frc/Filesystem.h
+++ b/wpilibc/src/main/native/include/frc/Filesystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -34,7 +34,7 @@
  * Obtains the deploy directory of the program, which is the remote location
  * src/main/deploy is deployed to by default. On the roboRIO, this is
  * /home/lvuser/deploy. In simulation, it is where the simulation was launched
- * from, in the subdirectory "deploy" (`pwd`/deploy).
+ * from, in the subdirectory "src/main/deploy" (`pwd`/src/main/deploy).
  *
  * @param result The result of the operating directory lookup
  */

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

diff --git a/wpilibc/src/main/native/include/frc/GearTooth.h b/wpilibc/src/main/native/include/frc/GearTooth.h
index 1c3df5b..2d1f792 100644
--- a/wpilibc/src/main/native/include/frc/GearTooth.h
+++ b/wpilibc/src/main/native/include/frc/GearTooth.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,6 +22,8 @@
  * Implements the gear tooth sensor supplied by FIRST. Currently there is no
  * reverse sensing on the gear tooth sensor, but in future versions we might
  * implement the necessary timing in the FPGA to sense direction.
+ *
+ * @deprecated No longer used per FMS usage reporting
  */
 class GearTooth : public Counter {
  public:

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

diff --git a/wpilibc/src/main/native/include/frc/GenericHID.h b/wpilibc/src/main/native/include/frc/GenericHID.h
index aae10cf..001b984 100644
--- a/wpilibc/src/main/native/include/frc/GenericHID.h
+++ b/wpilibc/src/main/native/include/frc/GenericHID.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -62,6 +62,9 @@
    * the state of each button. The appropriate button is returned as a boolean
    * value.
    *
+   * This method returns true if the button is being held down at the time
+   * that this method is being called.
+   *
    * @param button The button number to be read (starting at 1)
    * @return The state of the button.
    */
@@ -71,6 +74,10 @@
    * Whether the button was pressed since the last check. Button indexes begin
    * at 1.
    *
+   * This method returns true if the button went from not pressed to held down
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you press the button.
+   *
    * @param button The button index, beginning at 1.
    * @return Whether the button was pressed since the last check.
    */
@@ -80,6 +87,10 @@
    * Whether the button was released since the last check. Button indexes begin
    * at 1.
    *
+   * This method returns true if the button went from held down to not pressed
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you release the button.
+   *
    * @param button The button index, beginning at 1.
    * @return Whether the button was released since the last check.
    */
@@ -126,6 +137,13 @@
   int GetButtonCount() const;
 
   /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  bool IsConnected() const;
+
+  /**
    * Get the type of the HID.
    *
    * @return the type of the HID.

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

diff --git a/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h b/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
index 8c2a564..42b7434 100644
--- a/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
+++ b/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -115,9 +115,8 @@
   /**
    * Return the timestamp for the rising interrupt that occurred most recently.
    *
-   * This is in the same time domain as GetClock().
-   * The rising-edge interrupt should be enabled with
-   * {@link #DigitalInput.SetUpSourceEdge}
+   * This is in the same time domain as GetClock(). The rising-edge interrupt
+   * should be enabled with SetUpSourceEdge().
    *
    * @return Timestamp in seconds since boot.
    */

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

diff --git a/wpilibc/src/main/native/include/frc/IterativeRobot.h b/wpilibc/src/main/native/include/frc/IterativeRobot.h
index 447c477..24fdba3 100644
--- a/wpilibc/src/main/native/include/frc/IterativeRobot.h
+++ b/wpilibc/src/main/native/include/frc/IterativeRobot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,6 +21,9 @@
  *
  * Periodic() functions from the base class are called each time a new packet is
  * received from the driver station.
+ *
+ * @deprecated Use TimedRobot instead. It's a drop-in replacement that provides
+ * more regular execution periods.
  */
 class IterativeRobot : public IterativeRobotBase {
  public:
@@ -33,7 +36,7 @@
   /**
    * Provide an alternate "main loop" via StartCompetition().
    *
-   * This specific StartCompetition() implements "main loop" behaviour synced
+   * This specific StartCompetition() implements "main loop" behavior synced
    * with the DS packets.
    */
   void StartCompetition() override;

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

diff --git a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
index b78765a..66897aa 100644
--- a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
+++ b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/deprecated.h>
 
 #include "frc/RobotBase.h"
@@ -62,6 +62,16 @@
   virtual void RobotInit();
 
   /**
+   * Robot-wide simulation initialization code should go here.
+   *
+   * Users should override this method for default Robot-wide simulation
+   * related initialization which will be called when the robot is first
+   * started. It will be called exactly one time after RobotInit is called
+   * only when the robot is in simulation.
+   */
+  virtual void SimulationInit();
+
+  /**
    * Initialization code for disabled mode should go here.
    *
    * Users should override this method for initialization code which will be
@@ -103,6 +113,13 @@
   virtual void RobotPeriodic();
 
   /**
+   * Periodic simulation code should go here.
+   *
+   * This function is called in a simulated robot after user code executes.
+   */
+  virtual void SimulationPeriodic();
+
+  /**
    * Periodic code for disabled mode should go here.
    *
    * Users should override this method for code which will be called each time a
@@ -138,13 +155,15 @@
    */
   virtual void TestPeriodic();
 
- protected:
   /**
    * Constructor for IterativeRobotBase.
    *
    * @param period Period in seconds.
+   *
+   * @deprecated Use IterativeRobotBase(units::second_t period) with unit-safety
+   * instead
    */
-  WPI_DEPRECATED("Use ctor with unit-safety instead.")
+  WPI_DEPRECATED("Use constructor with unit-safety instead.")
   explicit IterativeRobotBase(double period);
 
   /**
@@ -156,6 +175,7 @@
 
   virtual ~IterativeRobotBase() = default;
 
+ protected:
   IterativeRobotBase(IterativeRobotBase&&) = default;
   IterativeRobotBase& operator=(IterativeRobotBase&&) = default;
 

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

diff --git a/wpilibc/src/main/native/include/frc/Joystick.h b/wpilibc/src/main/native/include/frc/Joystick.h
index 4ae398c..0975e6d 100644
--- a/wpilibc/src/main/native/include/frc/Joystick.h
+++ b/wpilibc/src/main/native/include/frc/Joystick.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,8 +9,6 @@
 
 #include <array>
 
-#include <wpi/deprecated.h>
-
 #include "frc/GenericHID.h"
 
 namespace frc {

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

diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h
deleted file mode 100644
index 2b83a32..0000000
--- a/wpilibc/src/main/native/include/frc/LinearFilter.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <cassert>
-#include <cmath>
-#include <initializer_list>
-#include <vector>
-
-#include <hal/FRCUsageReporting.h>
-#include <units/units.h>
-#include <wpi/ArrayRef.h>
-#include <wpi/circular_buffer.h>
-
-namespace frc {
-
-/**
- * This class implements a linear, digital filter. All types of FIR and IIR
- * filters are supported. Static factory methods are provided to create commonly
- * used types of filters.
- *
- * Filters are of the form:<br>
- *  y[n] = (b0 * x[n] + b1 * x[n-1] + … + bP * x[n-P]) -
- *         (a0 * y[n-1] + a2 * y[n-2] + … + aQ * y[n-Q])
- *
- * Where:<br>
- *  y[n] is the output at time "n"<br>
- *  x[n] is the input at time "n"<br>
- *  y[n-1] is the output from the LAST time step ("n-1")<br>
- *  x[n-1] is the input from the LAST time step ("n-1")<br>
- *  b0 … bP are the "feedforward" (FIR) gains<br>
- *  a0 … aQ are the "feedback" (IIR) gains<br>
- * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
- *            convention in signal processing.
- *
- * What can linear filters do? Basically, they can filter, or diminish, the
- * effects of undesirable input frequencies. High frequencies, or rapid changes,
- * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
- * filter smooths out the signal, reducing the impact of these high frequency
- * components.  Likewise, a "high pass" filter gets rid of slow-moving signal
- * components, letting you detect large changes more easily.
- *
- * Example FRC applications of filters:
- *  - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
- *    can do this faster in hardware)
- *  - Smoothing out joystick input to prevent the wheels from slipping or the
- *    robot from tipping
- *  - Smoothing motor commands so that unnecessary strain isn't put on
- *    electrical or mechanical components
- *  - If you use clever gains, you can make a PID controller out of this class!
- *
- * For more on filters, we highly recommend the following articles:<br>
- * https://en.wikipedia.org/wiki/Linear_filter<br>
- * https://en.wikipedia.org/wiki/Iir_filter<br>
- * https://en.wikipedia.org/wiki/Fir_filter<br>
- *
- * Note 1: Calculate() should be called by the user on a known, regular period.
- * You can use a Notifier for this or do it "inline" with code in a
- * periodic function.
- *
- * Note 2: For ALL filters, gains are necessarily a function of frequency. If
- * you make a filter that works well for you at, say, 100Hz, you will most
- * definitely need to adjust the gains if you then want to run it at 200Hz!
- * Combining this with Note 1 - the impetus is on YOU as a developer to make
- * sure Calculate() gets called at the desired, constant frequency!
- */
-template <class T>
-class LinearFilter {
- public:
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param ffGains The "feed forward" or FIR gains.
-   * @param fbGains The "feed back" or IIR gains.
-   */
-  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains)
-      : m_inputs(ffGains.size()),
-        m_outputs(fbGains.size()),
-        m_inputGains(ffGains),
-        m_outputGains(fbGains) {
-    static int instances = 0;
-    instances++;
-    HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
-  }
-
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param ffGains The "feed forward" or FIR gains.
-   * @param fbGains The "feed back" or IIR gains.
-   */
-  LinearFilter(std::initializer_list<double> ffGains,
-               std::initializer_list<double> fbGains)
-      : LinearFilter(wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
-                     wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
-
-  // Static methods to create commonly used filters
-  /**
-   * Creates a one-pole IIR low-pass filter of the form:<br>
-   *   y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
-   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
-   *
-   * This filter is stable for time constants greater than zero.
-   *
-   * @param timeConstant The discrete-time time constant in seconds.
-   * @param period       The period in seconds between samples taken by the
-   *                     user.
-   */
-  static LinearFilter<T> SinglePoleIIR(double timeConstant,
-                                       units::second_t period) {
-    double gain = std::exp(-period.to<double>() / timeConstant);
-    return LinearFilter(1.0 - gain, -gain);
-  }
-
-  /**
-   * Creates a first-order high-pass filter of the form:<br>
-   *   y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
-   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
-   *
-   * This filter is stable for time constants greater than zero.
-   *
-   * @param timeConstant The discrete-time time constant in seconds.
-   * @param period       The period in seconds between samples taken by the
-   *                     user.
-   */
-  static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
-    double gain = std::exp(-period.to<double>() / timeConstant);
-    return LinearFilter({gain, -gain}, {-gain});
-  }
-
-  /**
-   * Creates a K-tap FIR moving average filter of the form:<br>
-   *   y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
-   *
-   * This filter is always stable.
-   *
-   * @param taps The number of samples to average over. Higher = smoother but
-   *             slower
-   */
-  static LinearFilter<T> MovingAverage(int taps) {
-    assert(taps > 0);
-
-    std::vector<double> gains(taps, 1.0 / taps);
-    return LinearFilter(gains, {});
-  }
-
-  /**
-   * Reset the filter state.
-   */
-  void Reset() {
-    m_inputs.reset();
-    m_outputs.reset();
-  }
-
-  /**
-   * Calculates the next value of the filter.
-   *
-   * @param input Current input value.
-   *
-   * @return The filtered value at this step
-   */
-  T Calculate(T input) {
-    T retVal = T(0.0);
-
-    // Rotate the inputs
-    m_inputs.push_front(input);
-
-    // Calculate the new value
-    for (size_t i = 0; i < m_inputGains.size(); i++) {
-      retVal += m_inputs[i] * m_inputGains[i];
-    }
-    for (size_t i = 0; i < m_outputGains.size(); i++) {
-      retVal -= m_outputs[i] * m_outputGains[i];
-    }
-
-    // Rotate the outputs
-    m_outputs.push_front(retVal);
-
-    return retVal;
-  }
-
- private:
-  wpi::circular_buffer<T> m_inputs;
-  wpi::circular_buffer<T> m_outputs;
-  std::vector<double> m_inputGains;
-  std::vector<double> m_outputGains;
-};
-
-}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/MedianFilter.h b/wpilibc/src/main/native/include/frc/MedianFilter.h
deleted file mode 100644
index b5d499b..0000000
--- a/wpilibc/src/main/native/include/frc/MedianFilter.h
+++ /dev/null
@@ -1,80 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <algorithm>
-#include <vector>
-
-#include <wpi/Algorithm.h>
-#include <wpi/circular_buffer.h>
-
-namespace frc {
-/**
- * A class that implements a moving-window median filter.  Useful for reducing
- * measurement noise, especially with processes that generate occasional,
- * extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
- * sensors).
- */
-template <class T>
-class MedianFilter {
- public:
-  /**
-   * Creates a new MedianFilter.
-   *
-   * @param size The number of samples in the moving window.
-   */
-  explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
-
-  /**
-   * Calculates the moving-window median for the next value of the input stream.
-   *
-   * @param next The next input value.
-   * @return The median of the moving window, updated to include the next value.
-   */
-  T Calculate(T next) {
-    // Insert next value at proper point in sorted array
-    wpi::insert_sorted(m_orderedValues, next);
-
-    size_t curSize = m_orderedValues.size();
-
-    // If buffer is at max size, pop element off of end of circular buffer
-    // and remove from ordered list
-    if (curSize > m_size) {
-      m_orderedValues.erase(std::find(m_orderedValues.begin(),
-                                      m_orderedValues.end(),
-                                      m_valueBuffer.pop_back()));
-      curSize = curSize - 1;
-    }
-
-    // Add next value to circular buffer
-    m_valueBuffer.push_front(next);
-
-    if (curSize % 2 == 1) {
-      // If size is odd, return middle element of sorted list
-      return m_orderedValues[curSize / 2];
-    } else {
-      // If size is even, return average of middle elements
-      return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
-             2.0;
-    }
-  }
-
-  /**
-   * Resets the filter, clearing the window of all elements.
-   */
-  void Reset() {
-    m_orderedValues.clear();
-    m_valueBuffer.reset();
-  }
-
- private:
-  wpi::circular_buffer<T> m_valueBuffer;
-  std::vector<T> m_orderedValues;
-  size_t m_size;
-};
-}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h
index 24ffba3..c9348a6 100644
--- a/wpilibc/src/main/native/include/frc/Notifier.h
+++ b/wpilibc/src/main/native/include/frc/Notifier.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,10 +12,11 @@
 #include <atomic>
 #include <functional>
 #include <thread>
+#include <type_traits>
 #include <utility>
 
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/Twine.h>
 #include <wpi/deprecated.h>
 #include <wpi/mutex.h>
@@ -34,12 +35,34 @@
    */
   explicit Notifier(std::function<void()> handler);
 
-  template <typename Callable, typename Arg, typename... Args>
+  template <
+      typename Callable, typename Arg, typename... Args,
+      typename = std::enable_if_t<std::is_invocable_v<Callable, Arg, Args...>>>
   Notifier(Callable&& f, Arg&& arg, Args&&... args)
       : Notifier(std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
                            std::forward<Args>(args)...)) {}
 
   /**
+   * Create a Notifier for timer event notification.
+   *
+   * This overload makes the underlying thread run with a real-time priority.
+   * This is useful for reducing scheduling jitter on processes which are
+   * sensitive to timing variance, like model-based control.
+   *
+   * @param priority The FIFO real-time scheduler priority ([0..100] where a
+   *                 lower number represents higher priority).
+   * @param handler  The handler is called at the notification time which is set
+   *                 using StartSingle or StartPeriodic.
+   */
+  explicit Notifier(int priority, std::function<void()> handler);
+
+  template <typename Callable, typename Arg, typename... Args>
+  Notifier(int priority, Callable&& f, Arg&& arg, Args&&... args)
+      : Notifier(priority,
+                 std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
+                           std::forward<Args>(args)...)) {}
+
+  /**
    * Free the resources for a timer event.
    */
   virtual ~Notifier();
@@ -66,6 +89,9 @@
    *
    * A timer event is queued for a single event after the specified delay.
    *
+   * @deprecated Use unit-safe StartSingle(units::second_t delay) method
+   * instead.
+   *
    * @param delay Seconds to wait before the handler is called.
    */
   WPI_DEPRECATED("Use unit-safe StartSingle method instead.")
@@ -87,6 +113,9 @@
    * interrupt occurs, the event will be immediately requeued for the same time
    * interval.
    *
+   * @deprecated Use unit-safe StartPeriodic(units::second_t period) method
+   * instead
+   *
    * @param period Period in seconds to call the handler starting one period
    *               after the call to this method.
    */
@@ -106,9 +135,9 @@
   void StartPeriodic(units::second_t period);
 
   /**
-   * Stop timer events from occuring.
+   * Stop timer events from occurring.
    *
-   * Stop any repeating timer events from occuring. This will also remove any
+   * Stop any repeating timer events from occurring. This will also remove any
    * single notification events from the queue.
    *
    * If a timer-based call to the registered handler is in progress, this

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

diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h
index 513d46b..79d8eba 100644
--- a/wpilibc/src/main/native/include/frc/PIDBase.h
+++ b/wpilibc/src/main/native/include/frc/PIDBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -35,6 +35,8 @@
  * This feedback controller runs in discrete time, so time deltas are not used
  * in the integral and derivative calculations. Therefore, the sample rate
  * affects the controller's behavior for a given set of PID constants.
+ *
+ * @deprecated All APIs which use this have been deprecated.
  */
 class PIDBase : public PIDInterface,
                 public PIDOutput,

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

diff --git a/wpilibc/src/main/native/include/frc/PIDController.h b/wpilibc/src/main/native/include/frc/PIDController.h
index 88b0786..39e4deb 100644
--- a/wpilibc/src/main/native/include/frc/PIDController.h
+++ b/wpilibc/src/main/native/include/frc/PIDController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -33,6 +33,8 @@
  * This feedback controller runs in discrete time, so time deltas are not used
  * in the integral and derivative calculations. Therefore, the sample rate
  * affects the controller's behavior for a given set of PID constants.
+ *
+ * @deprecated Use frc2::PIDController class instead.
  */
 class PIDController : public PIDBase, public Controller {
  public:
@@ -46,7 +48,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, PIDSource* source,
@@ -62,7 +64,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, double f, PIDSource* source,
@@ -78,7 +80,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, PIDSource& source,
@@ -94,7 +96,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, double f, PIDSource& source,

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

diff --git a/wpilibc/src/main/native/include/frc/PIDInterface.h b/wpilibc/src/main/native/include/frc/PIDInterface.h
index 8162aa5..8d847a6 100644
--- a/wpilibc/src/main/native/include/frc/PIDInterface.h
+++ b/wpilibc/src/main/native/include/frc/PIDInterface.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,11 @@
 
 namespace frc {
 
+/**
+ * Interface for PID Control Loop.
+ *
+ * @deprecated All APIs which use this have been deprecated.
+ */
 class PIDInterface {
  public:
   WPI_DEPRECATED("All APIs which use this have been deprecated.")

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

diff --git a/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h b/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
index 433874b..9ec00a3 100644
--- a/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
+++ b/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -85,10 +85,16 @@
    */
   void ClearStickyFaults();
 
+  /**
+   * Gets module number (CAN ID).
+   */
+  int GetModule() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  private:
   hal::Handle<HAL_PDPHandle> m_handle;
+  int m_module;
 };
 
 }  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/Preferences.h b/wpilibc/src/main/native/include/frc/Preferences.h
index 57678a8..f04b012 100644
--- a/wpilibc/src/main/native/include/frc/Preferences.h
+++ b/wpilibc/src/main/native/include/frc/Preferences.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -122,6 +122,12 @@
   void PutString(wpi::StringRef key, wpi::StringRef value);
 
   /**
+   * Puts the given string into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitString(wpi::StringRef key, wpi::StringRef value);
+
+  /**
    * Puts the given int into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -132,6 +138,12 @@
   void PutInt(wpi::StringRef key, int value);
 
   /**
+   * Puts the given int into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitInt(wpi::StringRef key, int value);
+
+  /**
    * Puts the given double into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -142,6 +154,12 @@
   void PutDouble(wpi::StringRef key, double value);
 
   /**
+   * Puts the given double into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitDouble(wpi::StringRef key, double value);
+
+  /**
    * Puts the given float into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -152,6 +170,12 @@
   void PutFloat(wpi::StringRef key, float value);
 
   /**
+   * Puts the given float into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitFloat(wpi::StringRef key, float value);
+
+  /**
    * Puts the given boolean into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -162,6 +186,12 @@
   void PutBoolean(wpi::StringRef key, bool value);
 
   /**
+   * Puts the given boolean into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitBoolean(wpi::StringRef key, bool value);
+
+  /**
    * Puts the given long (int64_t) into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -172,6 +202,12 @@
   void PutLong(wpi::StringRef key, int64_t value);
 
   /**
+   * Puts the given long into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitLong(wpi::StringRef key, int64_t value);
+
+  /**
    * Returns whether or not there is a key with the given name.
    *
    * @param key the key

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

diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h
index 725aa97..66d5637 100644
--- a/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,7 @@
 #include <chrono>
 #include <thread>
 
+#include <hal/HALBase.h>
 #include <hal/Main.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
@@ -89,6 +90,8 @@
     impl::RunRobot<Robot>(m, &robot);
   }
 
+  HAL_Shutdown();
+
   return 0;
 }
 
@@ -134,6 +137,14 @@
   bool IsAutonomous() const;
 
   /**
+   * Determine if the robot is currently in Autonomous mode and enabled.
+   *
+   * @return True if the robot us currently operating Autonomously while enabled
+   * as determined by the field controls.
+   */
+  bool IsAutonomousEnabled() const;
+
+  /**
    * Determine if the robot is currently in Operator Control mode.
    *
    * @return True if the robot is currently operating in Tele-Op mode as
@@ -142,6 +153,14 @@
   bool IsOperatorControl() const;
 
   /**
+   * Determine if the robot is current in Operator Control mode and enabled.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while
+   * wnabled as determined by the field-controls.
+   */
+  bool IsOperatorControlEnabled() const;
+
+  /**
    * Determine if the robot is currently in Test mode.
    *
    * @return True if the robot is currently running tests as determined by the
@@ -166,6 +185,11 @@
 
   virtual void EndCompetition() = 0;
 
+  /**
+   * Get if the robot is real.
+   *
+   * @return If the robot is running in the real world.
+   */
   static constexpr bool IsReal() {
 #ifdef __FRC_ROBORIO__
     return true;
@@ -174,9 +198,13 @@
 #endif
   }
 
+  /**
+   * Get if the robot is a simulation.
+   *
+   * @return If the robot is running in simulation.
+   */
   static constexpr bool IsSimulation() { return !IsReal(); }
 
- protected:
   /**
    * Constructor for a generic robot program.
    *
@@ -192,6 +220,7 @@
 
   virtual ~RobotBase();
 
+ protected:
   // m_ds isn't moved in these because DriverStation is a singleton; every
   // instance of RobotBase has a reference to the same object.
   RobotBase(RobotBase&&) noexcept;

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

diff --git a/wpilibc/src/main/native/include/frc/RobotDrive.h b/wpilibc/src/main/native/include/frc/RobotDrive.h
index 3580003..a8b63e6 100644
--- a/wpilibc/src/main/native/include/frc/RobotDrive.h
+++ b/wpilibc/src/main/native/include/frc/RobotDrive.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -30,6 +30,9 @@
  * supplied on creation of the class. Those are used for either the Drive
  * function (intended for hand created drive code, such as autonomous) or with
  * the Tank/Arcade functions intended to be used for Operator Control driving.
+ *
+ * @deprecated Use DifferentialDrive or MecanumDrive classes instead.
+ *
  */
 class RobotDrive : public MotorSafety {
  public:
@@ -346,7 +349,7 @@
   /**
    * Holonomic Drive method for Mecanum wheeled robots.
    *
-   * This is an alias to MecanumDrive_Polar() for backward compatability
+   * This is an alias to MecanumDrive_Polar() for backward compatibility
    *
    * @param magnitude The speed that the robot should drive in a given
    *                  direction. [-1.0..1.0]

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

diff --git a/wpilibc/src/main/native/include/frc/RobotState.h b/wpilibc/src/main/native/include/frc/RobotState.h
index 60e608a..5f1c136 100644
--- a/wpilibc/src/main/native/include/frc/RobotState.h
+++ b/wpilibc/src/main/native/include/frc/RobotState.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,6 @@
 
 #pragma once
 
-#include <memory>
-
 namespace frc {
 
 class RobotState {

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

diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h
index 9bc9564..3592775 100644
--- a/wpilibc/src/main/native/include/frc/SPI.h
+++ b/wpilibc/src/main/native/include/frc/SPI.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 #include <memory>
 
 #include <hal/SPITypes.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/deprecated.h>
 
@@ -82,15 +82,21 @@
   /**
    * Configure that the data is stable on the falling edge and the data
    * changes on the rising edge.
+   *
+   * @deprecated Use SetSampleDataOnTrailingEdge() instead.
+   *
    */
-  WPI_DEPRECATED("Use SetSampleDataOnTrailingEdge in most cases.")
+  WPI_DEPRECATED("Use SetSampleDataOnTrailingEdge instead.")
   void SetSampleDataOnFalling();
 
   /**
    * Configure that the data is stable on the rising edge and the data
    * changes on the falling edge.
+   *
+   * @deprecated Use SetSampleDataOnLeadingEdge() instead.
+   *
    */
-  WPI_DEPRECATED("Use SetSampleDataOnLeadingEdge in most cases")
+  WPI_DEPRECATED("Use SetSampleDataOnLeadingEdge instead")
   void SetSampleDataOnRising();
 
   /**
@@ -116,11 +122,11 @@
   void SetChipSelectActiveLow();
 
   /**
-   * Write data to the slave device.  Blocks until there is space in the
+   * Write data to the peripheral device.  Blocks until there is space in the
    * output FIFO.
    *
    * If not running in output only mode, also saves the data received
-   * on the MISO input during the transfer into the receive FIFO.
+   * on the CIPO input during the transfer into the receive FIFO.
    */
   virtual int Write(uint8_t* data, int size);
 
@@ -166,10 +172,10 @@
   /**
    * Set the data to be transmitted by the engine.
    *
-   * Up to 23 bytes are configurable, and may be followed by up to 127 zero
+   * Up to 16 bytes are configurable, and may be followed by up to 127 zero
    * bytes.
    *
-   * @param dataToSend data to send (maximum 23 bytes)
+   * @param dataToSend data to send (maximum 16 bytes)
    * @param zeroSize number of zeros to send after the data
    */
   void SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize);
@@ -190,6 +196,8 @@
    * InitAuto() and SetAutoTransmitData() must be called before calling this
    * function.
    *
+   * @deprecated use unit-safe StartAutoRate(units::second_t period) instead.
+   *
    * @param period period between transfers, in seconds (us resolution)
    */
   WPI_DEPRECATED("Use StartAutoRate with unit-safety instead")
@@ -253,6 +261,10 @@
    * Blocks until numToRead words have been read or timeout expires.
    * May be called with numToRead=0 to retrieve how many words are available.
    *
+   * @deprecated Use unit safe version instead.
+   *             ReadAutoReceivedData(uint32_t* buffer, int numToRead, <!--
+   * -->         units::second_t timeout)
+   *
    * @param buffer buffer where read words are stored
    * @param numToRead number of words to read
    * @param timeout timeout in seconds (ms resolution)
@@ -304,6 +316,11 @@
   /**
    * Initialize the accumulator.
    *
+   * @deprecated Use unit-safe version instead.
+   *             InitAccumulator(units::second_t period, int cmd, int <!--
+   * -->         xferSize, int validMask, int validValue, int dataShift, <!--
+   * -->         int dataSize, bool isSigned, bool bigEndian)
+   *
    * @param period    Time between reads
    * @param cmd       SPI command to send to request data
    * @param xferSize  SPI transfer size, in bytes

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

diff --git a/wpilibc/src/main/native/include/frc/ScopedTracer.h b/wpilibc/src/main/native/include/frc/ScopedTracer.h
new file mode 100644
index 0000000..8634c1e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ScopedTracer.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+
+#include "frc/Tracer.h"
+
+namespace wpi {
+class raw_ostream;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * A class for keeping track of how much time it takes for different
+ * parts of code to execute. This class uses RAII, meaning you simply
+ * need to create an instance at the top of the block you are timing. After the
+ * block finishes execution (i.e. when the ScopedTracer instance gets
+ * destroyed), the epoch is printed to the provided raw_ostream.
+ */
+class ScopedTracer {
+ public:
+  /**
+   * Constructs a ScopedTracer instance.
+   *
+   * @param name The name of the epoch.
+   * @param os A reference to the raw_ostream to print data to.
+   */
+  ScopedTracer(wpi::Twine name, wpi::raw_ostream& os);
+  ~ScopedTracer();
+
+  ScopedTracer(const ScopedTracer&) = delete;
+  ScopedTracer& operator=(const ScopedTracer&) = delete;
+
+ private:
+  Tracer m_tracer;
+  std::string m_name;
+  wpi::raw_ostream& m_os;
+};
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/SerialPort.h b/wpilibc/src/main/native/include/frc/SerialPort.h
index f9edb84..7c44c75 100644
--- a/wpilibc/src/main/native/include/frc/SerialPort.h
+++ b/wpilibc/src/main/native/include/frc/SerialPort.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -106,7 +106,7 @@
    * Enable termination and specify the termination character.
    *
    * Termination is currently only implemented for receive.
-   * When the the terminator is recieved, the Read() or Scanf() will return
+   * When the the terminator is received, the Read() or Scanf() will return
    * fewer bytes than requested, stopping after the terminator.
    *
    * @param terminator The character to use for termination.
@@ -169,7 +169,7 @@
    *
    * Specify the amount of data that can be stored before data
    * from the device is returned to Read or Scanf.  If you want
-   * data that is recieved to be returned immediately, set this to 1.
+   * data that is received to be returned immediately, set this to 1.
    *
    * It the buffer is not filled before the read timeout expires, all
    * data that has been received so far will be returned.

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

diff --git a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
index 7236a30..d18fb09 100644
--- a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
+++ b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 #include <algorithm>
 
-#include <units/units.h>
+#include <units/time.h>
 
 namespace frc {
 /**
@@ -25,11 +25,11 @@
  */
 template <class Unit>
 class SlewRateLimiter {
+ public:
   using Unit_t = units::unit_t<Unit>;
   using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
   using Rate_t = units::unit_t<Rate>;
 
- public:
   /**
    * Creates a new SlewRateLimiter with the given rate limit and initial value.
    *
@@ -37,9 +37,9 @@
    * @param initialValue The initial value of the input.
    */
   explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
-      : m_rateLimit{rateLimit}, m_prevVal{initialValue} {
-    m_timer.Start();
-  }
+      : m_rateLimit{rateLimit},
+        m_prevVal{initialValue},
+        m_prevTime{frc2::Timer::GetFPGATimestamp()} {}
 
   /**
    * Filters the input to limit its slew rate.
@@ -49,9 +49,11 @@
    * rate.
    */
   Unit_t Calculate(Unit_t input) {
-    m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * m_timer.Get(),
-                            m_rateLimit * m_timer.Get());
-    m_timer.Reset();
+    units::second_t currentTime = frc2::Timer::GetFPGATimestamp();
+    units::second_t elapsedTime = currentTime - m_prevTime;
+    m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * elapsedTime,
+                            m_rateLimit * elapsedTime);
+    m_prevTime = currentTime;
     return m_prevVal;
   }
 
@@ -62,13 +64,13 @@
    * @param value The value to reset to.
    */
   void Reset(Unit_t value) {
-    m_timer.Reset();
     m_prevVal = value;
+    m_prevTime = frc2::Timer::GetFPGATimestamp();
   }
 
  private:
-  frc2::Timer m_timer;
   Rate_t m_rateLimit;
   Unit_t m_prevVal;
+  units::second_t m_prevTime;
 };
 }  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/Solenoid.h b/wpilibc/src/main/native/include/frc/Solenoid.h
index 86a2839..9939b41 100644
--- a/wpilibc/src/main/native/include/frc/Solenoid.h
+++ b/wpilibc/src/main/native/include/frc/Solenoid.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -62,6 +62,14 @@
   virtual bool Get() const;
 
   /**
+   * Toggle the value of the solenoid.
+   *
+   * If the solenoid is set to on, it'll be turned off. If the solenoid is set
+   * to off, it'll be turned on.
+   */
+  void Toggle();
+
+  /**
    * Check if solenoid is blacklisted.
    *
    * If a solenoid is shorted, it is added to the blacklist and

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

diff --git a/wpilibc/src/main/native/include/frc/SpeedController.h b/wpilibc/src/main/native/include/frc/SpeedController.h
index 2b11aee..3053c3b 100644
--- a/wpilibc/src/main/native/include/frc/SpeedController.h
+++ b/wpilibc/src/main/native/include/frc/SpeedController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/voltage.h>
 
 #include "frc/PIDOutput.h"
 

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

diff --git a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
index 80adf1c..e62563c 100644
--- a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
+++ b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,7 +23,8 @@
   template <class... SpeedControllers>
   explicit SpeedControllerGroup(SpeedController& speedController,
                                 SpeedControllers&... speedControllers);
-  ~SpeedControllerGroup() override = default;
+  explicit SpeedControllerGroup(
+      std::vector<std::reference_wrapper<SpeedController>>&& speedControllers);
 
   SpeedControllerGroup(SpeedControllerGroup&&) = default;
   SpeedControllerGroup& operator=(SpeedControllerGroup&&) = default;
@@ -41,6 +42,8 @@
  private:
   bool m_isInverted = false;
   std::vector<std::reference_wrapper<SpeedController>> m_speedControllers;
+
+  void Initialize();
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc

diff --git a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
index 5848746..d9e773d 100644
--- a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
+++ b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,19 +7,17 @@
 
 #pragma once
 
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <functional>
+#include <vector>
 
 namespace frc {
 
 template <class... SpeedControllers>
 SpeedControllerGroup::SpeedControllerGroup(
     SpeedController& speedController, SpeedControllers&... speedControllers)
-    : m_speedControllers{speedController, speedControllers...} {
-  for (auto& speedController : m_speedControllers)
-    SendableRegistry::GetInstance().AddChild(this, &speedController.get());
-  static int instances = 0;
-  ++instances;
-  SendableRegistry::GetInstance().Add(this, "SpeedControllerGroup", instances);
+    : m_speedControllers(std::vector<std::reference_wrapper<SpeedController>>{
+          speedController, speedControllers...}) {
+  Initialize();
 }
 
 }  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h
index 112e2c9..9dbc3f2 100644
--- a/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,12 +7,18 @@
 
 #pragma once
 
+#include <functional>
+#include <vector>
+
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/math.h>
+#include <units/time.h>
 #include <wpi/deprecated.h>
+#include <wpi/priority_queue.h>
 
 #include "frc/ErrorBase.h"
 #include "frc/IterativeRobotBase.h"
+#include "frc2/Timer.h"
 
 namespace frc {
 
@@ -47,6 +53,9 @@
   /**
    * Constructor for TimedRobot.
    *
+   * @deprecated use unit safe constructor instead.
+   * TimedRobot(units::second_t period = kDefaultPeriod)
+   *
    * @param period Period in seconds.
    */
   WPI_DEPRECATED("Use constructor with unit-safety instead.")
@@ -64,16 +73,57 @@
   TimedRobot(TimedRobot&&) = default;
   TimedRobot& operator=(TimedRobot&&) = default;
 
- private:
-  hal::Handle<HAL_NotifierHandle> m_notifier;
-
-  // The absolute expiration time
-  units::second_t m_expirationTime{0};
-
   /**
-   * Update the HAL alarm time.
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback
+   * run synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period   The period at which to run the callback.
+   * @param offset   The offset from the common starting time. This is useful
+   *                 for scheduling a callback in a different timeslot relative
+   *                 to TimedRobot.
    */
-  void UpdateAlarm();
+  void AddPeriodic(std::function<void()> callback, units::second_t period,
+                   units::second_t offset = 0_s);
+
+ private:
+  class Callback {
+   public:
+    std::function<void()> func;
+    units::second_t period;
+    units::second_t expirationTime;
+
+    /**
+     * Construct a callback container.
+     *
+     * @param func      The callback to run.
+     * @param startTime The common starting point for all callback scheduling.
+     * @param period    The period at which to run the callback.
+     * @param offset    The offset from the common starting time.
+     */
+    Callback(std::function<void()> func, units::second_t startTime,
+             units::second_t period, units::second_t offset)
+        : func{func},
+          period{period},
+          expirationTime{
+              startTime + offset +
+              units::math::floor((frc2::Timer::GetFPGATimestamp() - startTime) /
+                                 period) *
+                  period +
+              period} {}
+
+    bool operator>(const Callback& rhs) const {
+      return expirationTime > rhs.expirationTime;
+    }
+  };
+
+  hal::Handle<HAL_NotifierHandle> m_notifier;
+  units::second_t m_startTime;
+
+  wpi::priority_queue<Callback, std::vector<Callback>, std::greater<Callback>>
+      m_callbacks;
 };
 
 }  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/Timer.h b/wpilibc/src/main/native/include/frc/Timer.h
index 99caa47..ac166c5 100644
--- a/wpilibc/src/main/native/include/frc/Timer.h
+++ b/wpilibc/src/main/native/include/frc/Timer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,11 +7,6 @@
 
 #pragma once
 
-#include <units/units.h>
-#include <wpi/deprecated.h>
-#include <wpi/mutex.h>
-
-#include "frc/Base.h"
 #include "frc2/Timer.h"
 
 namespace frc {
@@ -132,9 +127,6 @@
    */
   static double GetMatchTime();
 
-  // The time, in seconds, at which the 32-bit FPGA timestamp rolls over to 0
-  static const double kRolloverTime;
-
  private:
   frc2::Timer m_timer;
 };

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

diff --git a/wpilibc/src/main/native/include/frc/Tracer.h b/wpilibc/src/main/native/include/frc/Tracer.h
new file mode 100644
index 0000000..627b07b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/Tracer.h
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <chrono>
+
+#include <hal/cpp/fpga_clock.h>
+#include <wpi/StringMap.h>
+#include <wpi/StringRef.h>
+
+namespace wpi {
+class raw_ostream;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * A class for keeping track of how much time it takes for different parts of
+ * code to execute. This is done with epochs, that are added to calls to
+ * AddEpoch() and can be printed with a call to PrintEpochs().
+ *
+ * Epochs are a way to partition the time elapsed so that when overruns occur,
+ * one can determine which parts of an operation consumed the most time.
+ */
+class Tracer {
+ public:
+  /**
+   * Constructs a Tracer instance.
+   */
+  Tracer();
+
+  /**
+   * Restarts the epoch timer.
+   */
+  void ResetTimer();
+
+  /**
+   * Clears all epochs.
+   */
+  void ClearEpochs();
+
+  /**
+   * Adds time since last epoch to the list printed by PrintEpochs().
+   *
+   * Epochs are a way to partition the time elapsed so that when overruns occur,
+   * one can determine which parts of an operation consumed the most time.
+   *
+   * @param epochName The name to associate with the epoch.
+   */
+  void AddEpoch(wpi::StringRef epochName);
+
+  /**
+   * Prints list of epochs added so far and their times to the DriverStation.
+   */
+  void PrintEpochs();
+
+  /**
+   * Prints list of epochs added so far and their times to a stream.
+   *
+   * @param os output stream
+   */
+  void PrintEpochs(wpi::raw_ostream& os);
+
+ private:
+  static constexpr std::chrono::milliseconds kMinPrintPeriod{1000};
+
+  hal::fpga_clock::time_point m_startTime;
+  hal::fpga_clock::time_point m_lastEpochsPrintTime = hal::fpga_clock::epoch();
+
+  wpi::StringMap<std::chrono::nanoseconds> m_epochs;
+};
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/Ultrasonic.h b/wpilibc/src/main/native/include/frc/Ultrasonic.h
index 637c6fc..f6d301b 100644
--- a/wpilibc/src/main/native/include/frc/Ultrasonic.h
+++ b/wpilibc/src/main/native/include/frc/Ultrasonic.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -132,7 +132,7 @@
    *                 the ultrasonic sensors. This scheduling method assures that
    *                 the sensors are non-interfering because no two sensors fire
    *                 at the same time. If another scheduling algorithm is
-   *                 prefered, it can be implemented by pinging the sensors
+   *                 preferred, it can be implemented by pinging the sensors
    *                 manually and waiting for the results to come back.
    */
   static void SetAutomaticMode(bool enabling);

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

diff --git a/wpilibc/src/main/native/include/frc/Utility.h b/wpilibc/src/main/native/include/frc/Utility.h
index 3caaf84..a02e583 100644
--- a/wpilibc/src/main/native/include/frc/Utility.h
+++ b/wpilibc/src/main/native/include/frc/Utility.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,13 +11,8 @@
  * @file Contains global utility functions
  */
 
-#include <stdint.h>
-
-#include <string>
-
 #include <wpi/StringRef.h>
 #include <wpi/Twine.h>
-#include <wpi/deprecated.h>
 
 #define wpi_assert(condition) \
   wpi_assert_impl(condition, #condition, "", __FILE__, __LINE__, __FUNCTION__)

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

diff --git a/wpilibc/src/main/native/include/frc/WPILib.h b/wpilibc/src/main/native/include/frc/WPILib.h
index 9d62514..634d0f5 100644
--- a/wpilibc/src/main/native/include/frc/WPILib.h
+++ b/wpilibc/src/main/native/include/frc/WPILib.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 // clang-format off
 #ifdef _MSC_VER
-#pragma message "warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead."
+#pragma message("warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead.")
 #else
 #warning "Including this header drastically increases compilation times and is bad style. Include only what you use instead."
 #endif

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

diff --git a/wpilibc/src/main/native/include/frc/Watchdog.h b/wpilibc/src/main/native/include/frc/Watchdog.h
index b36cf23..6fc3793 100644
--- a/wpilibc/src/main/native/include/frc/Watchdog.h
+++ b/wpilibc/src/main/native/include/frc/Watchdog.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,17 +7,15 @@
 
 #pragma once
 
-#include <chrono>
 #include <functional>
 #include <utility>
 
-#include <hal/cpp/fpga_clock.h>
-#include <units/units.h>
-#include <wpi/SafeThread.h>
-#include <wpi/StringMap.h>
+#include <units/time.h>
 #include <wpi/StringRef.h>
 #include <wpi/deprecated.h>
 
+#include "frc/Tracer.h"
+
 namespace frc {
 
 /**
@@ -34,6 +32,9 @@
   /**
    * Watchdog constructor.
    *
+   * @deprecated use unit-safe version instead.
+   * Watchdog(units::second_t timeout, std::function<void()> callback)
+   *
    * @param timeout  The watchdog's timeout in seconds with microsecond
    *                 resolution.
    * @param callback This function is called when the timeout expires.
@@ -63,8 +64,8 @@
 
   ~Watchdog();
 
-  Watchdog(Watchdog&&) = default;
-  Watchdog& operator=(Watchdog&&) = default;
+  Watchdog(Watchdog&& rhs);
+  Watchdog& operator=(Watchdog&& rhs);
 
   /**
    * Returns the time in seconds since the watchdog was last fed.
@@ -74,6 +75,9 @@
   /**
    * Sets the watchdog's timeout.
    *
+   * @deprecated use the unit safe version instead.
+   * SetTimeout(units::second_t timeout)
+   *
    * @param timeout The watchdog's timeout in seconds with microsecond
    *                resolution.
    */
@@ -142,26 +146,25 @@
 
  private:
   // Used for timeout print rate-limiting
-  static constexpr std::chrono::milliseconds kMinPrintPeriod{1000};
+  static constexpr units::second_t kMinPrintPeriod = 1_s;
 
-  hal::fpga_clock::time_point m_startTime;
-  std::chrono::nanoseconds m_timeout;
-  hal::fpga_clock::time_point m_expirationTime;
+  units::second_t m_startTime = 0_s;
+  units::second_t m_timeout;
+  units::second_t m_expirationTime = 0_s;
   std::function<void()> m_callback;
-  hal::fpga_clock::time_point m_lastTimeoutPrintTime = hal::fpga_clock::epoch();
-  hal::fpga_clock::time_point m_lastEpochsPrintTime = hal::fpga_clock::epoch();
+  units::second_t m_lastTimeoutPrintTime = 0_s;
 
-  wpi::StringMap<std::chrono::nanoseconds> m_epochs;
+  Tracer m_tracer;
   bool m_isExpired = false;
 
   bool m_suppressTimeoutMessage = false;
 
-  class Thread;
-  wpi::SafeThreadOwner<Thread>* m_owner;
+  class Impl;
+  Impl* m_impl;
 
-  bool operator>(const Watchdog& rhs);
+  bool operator>(const Watchdog& rhs) const;
 
-  static wpi::SafeThreadOwner<Thread>& GetThreadOwner();
+  static Impl* GetImpl();
 };
 
 }  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/XboxController.h b/wpilibc/src/main/native/include/frc/XboxController.h
index 3ca2f4b..ddf4dc2 100644
--- a/wpilibc/src/main/native/include/frc/XboxController.h
+++ b/wpilibc/src/main/native/include/frc/XboxController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -244,6 +244,15 @@
     kBack = 7,
     kStart = 8
   };
+
+  enum class Axis {
+    kLeftX = 0,
+    kRightX = 4,
+    kLeftY = 1,
+    kRightY = 5,
+    kLeftTrigger = 2,
+    kRightTrigger = 3
+  };
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h

diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
deleted file mode 100644
index 6f6e086..0000000
--- a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
+++ /dev/null
@@ -1,149 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-#include <wpi/MathExtras.h>
-
-namespace frc {
-/**
- * A helper class that computes feedforward outputs for a simple arm (modeled as
- * a motor acting against the force of gravity on a beam suspended at an angle).
- */
-class ArmFeedforward {
-  using Angle = units::radians;
-  using Velocity = units::radians_per_second;
-  using Acceleration = units::compound_unit<units::radians_per_second,
-                                            units::inverse<units::second>>;
-  using kv_unit =
-      units::compound_unit<units::volts,
-                           units::inverse<units::radians_per_second>>;
-  using ka_unit =
-      units::compound_unit<units::volts, units::inverse<Acceleration>>;
-
- public:
-  constexpr ArmFeedforward() = default;
-
-  /**
-   * Creates a new ArmFeedforward with the specified gains.
-   *
-   * @param kS   The static gain, in volts.
-   * @param kCos 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::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param angle        The angle setpoint, in radians.
-   * @param velocity     The velocity setpoint, in radians per second.
-   * @param acceleration The acceleration setpoint, in radians per second^2.
-   * @return The computed feedforward, in volts.
-   */
-  units::volt_t Calculate(units::unit_t<Angle> angle,
-                          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) +
-           kV * velocity + kA * acceleration;
-  }
-
-  // Rearranging the main equation from the calculate() method yields the
-  // formulas for the methods below:
-
-  /**
-   * Calculates the maximum achievable velocity given a maximum voltage supply,
-   * a position, and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
-   * @param acceleration The acceleration of the arm.
-   * @return The maximum possible velocity at the given acceleration and angle.
-   */
-  units::unit_t<Velocity> MaxAchievableVelocity(
-      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) -
-            kA * acceleration) /
-           kV;
-  }
-
-  /**
-   * Calculates the minimum achievable velocity given a maximum voltage supply,
-   * a position, and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
-   * @param acceleration The acceleration of the arm.
-   * @return The minimum possible velocity at the given acceleration and angle.
-   */
-  units::unit_t<Velocity> MinAchievableVelocity(
-      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) -
-            kA * acceleration) /
-           kV;
-  }
-
-  /**
-   * Calculates the maximum achievable acceleration given a maximum voltage
-   * supply, a position, 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
-   * @param velocity The velocity of the arm.
-   * @return The maximum possible acceleration at the given velocity and angle.
-   */
-  units::unit_t<Acceleration> MaxAchievableAcceleration(
-      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) /
-           kA;
-  }
-
-  /**
-   * Calculates the minimum achievable acceleration given a maximum voltage
-   * supply, a position, 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm
-   * @param velocity The velocity of the arm.
-   * @return The minimum possible acceleration at the given velocity and angle.
-   */
-  units::unit_t<Acceleration> MinAchievableAcceleration(
-      units::volt_t maxVoltage, units::unit_t<Angle> angle,
-      units::unit_t<Velocity> velocity) {
-    return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
-  }
-
-  units::volt_t kS{0};
-  units::volt_t kCos{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/controller/ControllerUtil.h

diff --git a/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
new file mode 100644
index 0000000..7603fde
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+#include <type_traits>
+
+#include <units/math.h>
+
+namespace frc {
+
+/**
+ * Returns modulus of error where error is the difference between the reference
+ * and a measurement.
+ *
+ * @param reference Reference input of a controller.
+ * @param measurement The current measurement.
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+template <typename T>
+T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) {
+  T error = reference - measurement;
+  T modulus = maximumInput - minimumInput;
+
+  // Wrap error above maximum input
+  int numMax = (error + maximumInput) / modulus;
+  error -= numMax * modulus;
+
+  // Wrap error below minimum input
+  int numMin = (error + minimumInput) / modulus;
+  error -= numMin * modulus;
+
+  return error;
+}
+
+}  // namespace frc

wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h

diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
deleted file mode 100644
index c664fc4..0000000
--- a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-#include <wpi/MathExtras.h>
-
-namespace frc {
-/**
- * A helper class that computes feedforward outputs for a simple elevator
- * (modeled as a motor acting against the force of gravity).
- */
-template <class Distance>
-class ElevatorFeedforward {
-  using Velocity =
-      units::compound_unit<Distance, units::inverse<units::seconds>>;
-  using Acceleration =
-      units::compound_unit<Velocity, units::inverse<units::seconds>>;
-  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
-  using ka_unit =
-      units::compound_unit<units::volts, units::inverse<Acceleration>>;
-
- public:
-  ElevatorFeedforward() = default;
-
-  /**
-   * Creates a new ElevatorFeedforward with the specified gains.
-   *
-   * @param kS The static gain, in volts.
-   * @param kG The gravity gain, in volts.
-   * @param kV The velocity gain, in volt seconds per distance.
-   * @param kA The acceleration gain, in volt seconds^2 per distance.
-   */
-  constexpr ElevatorFeedforward(
-      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), kG(kG), kV(kV), kA(kA) {}
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param velocity     The velocity setpoint, in distance per second.
-   * @param acceleration The acceleration setpoint, in distance per second^2.
-   * @return The computed feedforward, in volts.
-   */
-  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
-                                    units::unit_t<Acceleration> acceleration =
-                                        units::unit_t<Acceleration>(0)) {
-    return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
-  }
-
-  // Rearranging the main equation from the calculate() method yields the
-  // formulas for the methods below:
-
-  /**
-   * Calculates the maximum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param acceleration The acceleration of the elevator.
-   * @return The maximum possible velocity at the given acceleration.
-   */
-  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
-      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - kS - kG - kA * acceleration) / kV;
-  }
-
-  /**
-   * Calculates the minimum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param acceleration The acceleration of the elevator.
-   * @return The minimum possible velocity at the given acceleration.
-   */
-  constexpr units::unit_t<Velocity> MinAchievableVelocity(
-      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
-    // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + kS - kG - kA * acceleration) / kV;
-  }
-
-  /**
-   * Calculates the maximum 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param velocity The velocity of the elevator.
-   * @return The maximum possible acceleration at the given velocity.
-   */
-  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
-      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
-    return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
-  }
-
-  /**
-   * 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param velocity The velocity of the elevator.
-   * @return The minimum possible acceleration at the given velocity.
-   */
-  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
-      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
-    return MaxAchievableAcceleration(-maxVoltage, velocity);
-  }
-
-  units::volt_t kS{0};
-  units::volt_t kG{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h

diff --git a/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
new file mode 100644
index 0000000..916e1a9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/angle.h>
+#include <units/velocity.h>
+
+#include "frc/controller/PIDController.h"
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/trajectory/Trajectory.h"
+
+namespace frc {
+/**
+ * This holonomic drive controller can be used to follow trajectories using a
+ * holonomic drive train (i.e. swerve or mecanum). Holonomic trajectory
+ * following is a much simpler problem to solve compared to skid-steer style
+ * drivetrains because it is possible to individually control forward, sideways,
+ * and angular velocity.
+ *
+ * The holonomic drive controller takes in one PID controller for each
+ * direction, forward and sideways, and one profiled PID controller for the
+ * angular direction. Because the heading dynamics are decoupled from
+ * translations, users can specify a custom heading that the drivetrain should
+ * point toward. This heading reference is profiled for smoothness.
+ */
+class HolonomicDriveController {
+ public:
+  /**
+   * Constructs a holonomic drive controller.
+   *
+   * @param xController     A PID Controller to respond to error in the
+   * field-relative x direction.
+   * @param yController     A PID Controller to respond to error in the
+   * field-relative y direction.
+   * @param thetaController A profiled PID controller to respond to error in
+   * angle.
+   */
+  HolonomicDriveController(
+      const frc2::PIDController& xController,
+      const frc2::PIDController& yController,
+      const ProfiledPIDController<units::radian>& thetaController);
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& tolerance);
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose        The current pose.
+   * @param poseRef            The desired pose.
+   * @param linearVelocityRef  The desired linear velocity.
+   * @param angleRef           The desired ending angle.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          const Rotation2d& angleRef);
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   * @param angleRef     The desired ending angle.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Trajectory::State& desiredState,
+                          const Rotation2d& angleRef);
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes. When
+   * Calculate() is called on a disabled controller, only feedforward values
+   * are returned.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
+ private:
+  Pose2d m_poseError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+
+  frc2::PIDController m_xController;
+  frc2::PIDController m_yController;
+  ProfiledPIDController<units::radian> m_thetaController;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/controller/PIDController.h

diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h
index 66a36fb..5f97c1e 100644
--- a/wpilibc/src/main/native/include/frc/controller/PIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,7 @@
 #include <functional>
 #include <limits>
 
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableHelper.h"
@@ -117,7 +117,7 @@
   double GetSetpoint() const;
 
   /**
-   * Returns true if the error is within the tolerance of the error.
+   * Returns true if the error is within the tolerance of the setpoint.
    *
    * This will return false until at least one input value has been computed.
    */
@@ -141,6 +141,11 @@
   void DisableContinuousInput();
 
   /**
+   * Returns true if continuous input is enabled.
+   */
+  bool IsContinuousInputEnabled() const;
+
+  /**
    * Sets the minimum and maximum values for the integrator.
    *
    * When the cap is reached, the integrator value is added to the controller
@@ -193,16 +198,6 @@
 
   void InitSendable(frc::SendableBuilder& builder) override;
 
- protected:
-  /**
-   * Wraps error around for continuous inputs. The original error is returned if
-   * continuous mode is disabled.
-   *
-   * @param error The current error of the PID controller.
-   * @return Error for continuous inputs.
-   */
-  double GetContinuousError(double error) const;
-
  private:
   // Factor for "proportional" control
   double m_Kp;
@@ -220,15 +215,10 @@
 
   double m_minimumIntegral = -1.0;
 
-  // Maximum input - limit setpoint to this
   double m_maximumInput = 0;
 
-  // Minimum input - limit setpoint to this
   double m_minimumInput = 0;
 
-  // Input range - difference between maximum and minimum
-  double m_inputRange = 0;
-
   // Do the endpoints wrap around? eg. Absolute encoder
   bool m_continuous = false;
 
@@ -248,14 +238,6 @@
   double m_velocityTolerance = std::numeric_limits<double>::infinity();
 
   double m_setpoint = 0;
-
-  /**
-   * Sets the minimum and maximum values expected from the input.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  void SetInputRange(double minimumInput, double maximumInput);
 };
 
 }  // namespace frc2

wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h

diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 079d96e..fd246cd 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -12,8 +12,9 @@
 #include <functional>
 #include <limits>
 
-#include <units/units.h>
+#include <units/time.h>
 
+#include "frc/controller/ControllerUtil.h"
 #include "frc/controller/PIDController.h"
 #include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -33,6 +34,7 @@
 class ProfiledPIDController
     : public Sendable,
       public SendableHelper<ProfiledPIDController<Distance>> {
+ public:
   using Distance_t = units::unit_t<Distance>;
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
@@ -43,7 +45,6 @@
   using State = typename TrapezoidProfile<Distance>::State;
   using Constraints = typename TrapezoidProfile<Distance>::Constraints;
 
- public:
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
    * Kd. Users should call reset() when they first start running the controller
@@ -195,6 +196,8 @@
   void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
     m_controller.EnableContinuousInput(minimumInput.template to<double>(),
                                        maximumInput.template to<double>());
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -251,6 +254,23 @@
    * @param measurement The current measurement of the process variable.
    */
   double Calculate(Distance_t measurement) {
+    if (m_controller.IsContinuousInputEnabled()) {
+      // Get error which is smallest distance between goal and measurement
+      auto goalMinDistance = frc::GetModulusError<Distance_t>(
+          m_goal.position, measurement, m_minimumInput, m_maximumInput);
+      auto setpointMinDistance = frc::GetModulusError<Distance_t>(
+          m_setpoint.position, measurement, m_minimumInput, m_maximumInput);
+
+      // Recompute the profile goal with the smallest error, thus giving the
+      // shortest path. The goal may be outside the input range after this
+      // operation, but that's OK because the controller will still go there and
+      // report an error of zero. In other words, the setpoint only needs to be
+      // offset from the measurement by the input range modulus; they don't need
+      // to be equal.
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
+    }
+
     frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
     m_setpoint = profile.Calculate(GetPeriod());
     return m_controller.Calculate(measurement.template to<double>(),
@@ -324,12 +344,12 @@
 
   void InitSendable(frc::SendableBuilder& builder) override {
     builder.SetSmartDashboardType("ProfiledPIDController");
-    builder.AddDoubleProperty("p", [this] { return GetP(); },
-                              [this](double value) { SetP(value); });
-    builder.AddDoubleProperty("i", [this] { return GetI(); },
-                              [this](double value) { SetI(value); });
-    builder.AddDoubleProperty("d", [this] { return GetD(); },
-                              [this](double value) { SetD(value); });
+    builder.AddDoubleProperty(
+        "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
+    builder.AddDoubleProperty(
+        "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
+    builder.AddDoubleProperty(
+        "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
     builder.AddDoubleProperty(
         "goal", [this] { return GetGoal().position.template to<double>(); },
         [this](double value) { SetGoal(Distance_t{value}); });
@@ -337,6 +357,8 @@
 
  private:
   frc2::PIDController m_controller;
+  Distance_t m_minimumInput{0};
+  Distance_t m_maximumInput{0};
   typename frc::TrapezoidProfile<Distance>::State m_goal;
   typename frc::TrapezoidProfile<Distance>::State m_setpoint;
   typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;

wpilibc/src/main/native/include/frc/controller/RamseteController.h

diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
index 6ec6edc..e746bb3 100644
--- a/wpilibc/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,8 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/angular_velocity.h>
+#include <units/velocity.h>
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
@@ -56,7 +57,7 @@
 
   /**
    * Construct a Ramsete unicycle controller. The default arguments for
-   * b and zeta of 2.0 and 0.7 have been well-tested to produce desireable
+   * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
    * results.
    */
   RamseteController() : RamseteController(2.0, 0.7) {}
@@ -102,12 +103,20 @@
   ChassisSpeeds Calculate(const Pose2d& currentPose,
                           const Trajectory::State& desiredState);
 
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
  private:
   double m_b;
   double m_zeta;
 
   Pose2d m_poseError;
   Pose2d m_poseTolerance;
+  bool m_enabled = true;
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h

diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
deleted file mode 100644
index 8f82cb9..0000000
--- a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-#include <wpi/MathExtras.h>
-
-namespace frc {
-/**
- * A helper class that computes feedforward voltages for a simple
- * permanent-magnet DC motor.
- */
-template <class Distance>
-class SimpleMotorFeedforward {
-  using Velocity =
-      units::compound_unit<Distance, units::inverse<units::seconds>>;
-  using Acceleration =
-      units::compound_unit<Velocity, units::inverse<units::seconds>>;
-  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
-  using ka_unit =
-      units::compound_unit<units::volts, units::inverse<Acceleration>>;
-
- public:
-  constexpr SimpleMotorFeedforward() = default;
-
-  /**
-   * Creates a new SimpleMotorFeedforward with the specified gains.
-   *
-   * @param kS The static gain, in volts.
-   * @param kV The velocity gain, in volt seconds per distance.
-   * @param kA The acceleration gain, in volt seconds^2 per distance.
-   */
-  constexpr SimpleMotorFeedforward(
-      units::volt_t kS, units::unit_t<kv_unit> kV,
-      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
-      : kS(kS), kV(kV), kA(kA) {}
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param velocity     The velocity setpoint, in distance per second.
-   * @param acceleration The acceleration setpoint, in distance per second^2.
-   * @return The computed feedforward, in volts.
-   */
-  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
-                                    units::unit_t<Acceleration> acceleration =
-                                        units::unit_t<Acceleration>(0)) const {
-    return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
-  }
-
-  // Rearranging the main equation from the calculate() method yields the
-  // formulas for the methods below:
-
-  /**
-   * Calculates the maximum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param acceleration The acceleration of the motor.
-   * @return The maximum possible velocity at the given acceleration.
-   */
-  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
-      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - kS - kA * acceleration) / kV;
-  }
-
-  /**
-   * Calculates the minimum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param acceleration The acceleration of the motor.
-   * @return The minimum possible velocity at the given acceleration.
-   */
-  constexpr units::unit_t<Velocity> MinAchievableVelocity(
-      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
-    // Assume min velocity is positive, ks flips sign
-    return (-maxVoltage + kS - kA * acceleration) / kV;
-  }
-
-  /**
-   * Calculates the maximum 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param velocity The velocity of the motor.
-   * @return The maximum possible acceleration at the given velocity.
-   */
-  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
-      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
-    return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
-  }
-
-  /**
-   * 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param velocity The velocity of the motor.
-   * @return The minimum possible acceleration at the given velocity.
-   */
-  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
-      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
-    return MaxAchievableAcceleration(-maxVoltage, velocity);
-  }
-
-  units::volt_t kS{0};
-  units::unit_t<kv_unit> kV{0};
-  units::unit_t<ka_unit> kA{0};
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h

diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index c6b705a..86103de 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -191,7 +191,7 @@
   void SetQuickStopAlpha(double alpha);
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multipled by
+   * Gets if the power sent to the right side of the drivetrain is multiplied by
    * -1.
    *
    * @return true if the right side is inverted
@@ -200,9 +200,10 @@
 
   /**
    * Sets if the power sent to the right side of the drivetrain should be
-   * multipled by -1.
+   * multiplied by -1.
    *
-   * @param rightSideInverted true if right side power should be multipled by -1
+   * @param rightSideInverted true if right side power should be multiplied by
+   * -1
    */
   void SetRightSideInverted(bool rightSideInverted);
 

wpilibc/src/main/native/include/frc/drive/MecanumDrive.h

diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
index 9ddb57e..8435b4d 100644
--- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -116,7 +116,7 @@
   void DrivePolar(double magnitude, double angle, double zRotation);
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multipled by
+   * Gets if the power sent to the right side of the drivetrain is multiplied by
    * -1.
    *
    * @return true if the right side is inverted
@@ -125,9 +125,10 @@
 
   /**
    * Sets if the power sent to the right side of the drivetrain should be
-   * multipled by -1.
+   * multiplied by -1.
    *
-   * @param rightSideInverted true if right side power should be multipled by -1
+   * @param rightSideInverted true if right side power should be multiplied by
+   * -1
    */
   void SetRightSideInverted(bool rightSideInverted);
 

wpilibc/src/main/native/include/frc/filters/Filter.h

diff --git a/wpilibc/src/main/native/include/frc/filters/Filter.h b/wpilibc/src/main/native/include/frc/filters/Filter.h
index b200407..fb14d28 100644
--- a/wpilibc/src/main/native/include/frc/filters/Filter.h
+++ b/wpilibc/src/main/native/include/frc/filters/Filter.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,6 +17,8 @@
 
 /**
  * Interface for filters
+ *
+ * @deprecated only used by the deprecated LinearDigitalFilter
  */
 class Filter : public PIDSource {
  public:

wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h

diff --git a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
index 0a2afd2..c4fc3ef 100644
--- a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
+++ b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -68,6 +68,8 @@
  * definitely need to adjust the gains if you then want to run it at 200Hz!
  * Combining this with Note 1 - the impetus is on YOU as a developer to make
  * sure PIDGet() gets called at the desired, constant frequency!
+ *
+ * @deprecated Use LinearFilter class instead
  */
 class LinearDigitalFilter : public Filter {
  public:

wpilibc/src/main/native/include/frc/geometry/Pose2d.h

diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
deleted file mode 100644
index 46328a0..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
+++ /dev/null
@@ -1,179 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "Transform2d.h"
-#include "Translation2d.h"
-#include "Twist2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * Represents a 2d pose containing translational and rotational elements.
- */
-class Pose2d {
- public:
-  /**
-   * Constructs a pose at the origin facing toward the positive X axis.
-   * (Translation2d{0, 0} and Rotation{0})
-   */
-  constexpr Pose2d() = default;
-
-  /**
-   * Constructs a pose with the specified translation and rotation.
-   *
-   * @param translation The translational component of the pose.
-   * @param rotation The rotational component of the pose.
-   */
-  Pose2d(Translation2d translation, Rotation2d rotation);
-
-  /**
-   * Convenience constructors that takes in x and y values directly instead of
-   * having to construct a Translation2d.
-   *
-   * @param x The x component of the translational component of the pose.
-   * @param y The y component of the translational component of the pose.
-   * @param rotation The rotational component of the pose.
-   */
-  Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
-
-  /**
-   * Transforms the pose by the given transformation and returns the new
-   * transformed pose.
-   *
-   * [x_new]    [cos, -sin, 0][transform.x]
-   * [y_new] += [sin,  cos, 0][transform.y]
-   * [t_new]    [0,    0,   1][transform.t]
-   *
-   * @param other The transform to transform the pose by.
-   *
-   * @return The transformed pose.
-   */
-  Pose2d operator+(const Transform2d& other) const;
-
-  /**
-   * Transforms the current pose by the transformation.
-   *
-   * This is similar to the + operator, except that it mutates the current
-   * object.
-   *
-   * @param other The transform to transform the pose by.
-   *
-   * @return Reference to the new mutated object.
-   */
-  Pose2d& operator+=(const Transform2d& other);
-
-  /**
-   * Returns the Transform2d that maps the one pose to another.
-   *
-   * @param other The initial pose of the transformation.
-   * @return The transform that maps the other pose to the current pose.
-   */
-  Transform2d operator-(const Pose2d& other) const;
-
-  /**
-   * Checks equality between this Pose2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Pose2d& other) const;
-
-  /**
-   * Checks inequality between this Pose2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Pose2d& other) const;
-
-  /**
-   * Returns the underlying translation.
-   *
-   * @return Reference to the translational component of the pose.
-   */
-  const Translation2d& Translation() const { return m_translation; }
-
-  /**
-   * Returns the underlying rotation.
-   *
-   * @return Reference to the rotational component of the pose.
-   */
-  const Rotation2d& Rotation() const { return m_rotation; }
-
-  /**
-   * Transforms the pose by the given transformation and returns the new pose.
-   * See + operator for the matrix multiplication performed.
-   *
-   * @param other The transform to transform the pose by.
-   *
-   * @return The transformed pose.
-   */
-  Pose2d TransformBy(const Transform2d& other) const;
-
-  /**
-   * Returns the other pose relative to the current pose.
-   *
-   * This function can often be used for trajectory tracking or pose
-   * stabilization algorithms to get the error between the reference and the
-   * current pose.
-   *
-   * @param other The pose that is the origin of the new coordinate frame that
-   * the current pose will be converted into.
-   *
-   * @return The current pose relative to the new origin pose.
-   */
-  Pose2d RelativeTo(const Pose2d& other) const;
-
-  /**
-   * Obtain a new Pose2d from a (constant curvature) velocity.
-   *
-   * See <https://file.tavsys.net/control/state-space-guide.pdf> section on
-   * nonlinear pose estimation for derivation.
-   *
-   * The twist is a change in pose in the robot's coordinate frame since the
-   * previous pose update. When the user runs exp() on the previous known
-   * field-relative pose with the argument being the twist, the user will
-   * receive the new field-relative pose.
-   *
-   * "Exp" represents the pose exponential, which is solving a differential
-   * equation moving the pose forward in time.
-   *
-   * @param twist The change in pose in the robot's coordinate frame since the
-   * previous pose update. For example, if a non-holonomic robot moves forward
-   * 0.01 meters and changes angle by 0.5 degrees since the previous pose
-   * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
-   *
-   * @return The new pose of the robot.
-   */
-  Pose2d Exp(const Twist2d& twist) const;
-
-  /**
-   * Returns a Twist2d that maps this pose to the end pose. If c is the output
-   * of a.Log(b), then a.Exp(c) would yield b.
-   *
-   * @param end The end pose for the transformation.
-   *
-   * @return The twist that maps this to end.
-   */
-  Twist2d Log(const Pose2d& end) const;
-
- private:
-  Translation2d m_translation;
-  Rotation2d m_rotation;
-};
-
-void to_json(wpi::json& json, const Pose2d& pose);
-
-void from_json(const wpi::json& json, Pose2d& pose);
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/geometry/Rotation2d.h

diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
deleted file mode 100644
index b636513..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
+++ /dev/null
@@ -1,186 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * A rotation in a 2d coordinate frame represented a point on the unit circle
- * (cosine and sine).
- */
-class Rotation2d {
- public:
-  /**
-   * Constructs a Rotation2d with a default angle of 0 degrees.
-   */
-  constexpr Rotation2d() = default;
-
-  /**
-   * Constructs a Rotation2d with the given radian value.
-   *
-   * @param value The value of the angle in radians.
-   */
-  Rotation2d(units::radian_t value);  // NOLINT(runtime/explicit)
-
-  /**
-   * Constructs a Rotation2d with the given x and y (cosine and sine)
-   * components. The x and y don't have to be normalized.
-   *
-   * @param x The x component or cosine of the rotation.
-   * @param y The y component or sine of the rotation.
-   */
-  Rotation2d(double x, double y);
-
-  /**
-   * Adds two rotations together, with the result being bounded between -pi and
-   * pi.
-   *
-   * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to add.
-   *
-   * @return The sum of the two rotations.
-   */
-  Rotation2d operator+(const Rotation2d& other) const;
-
-  /**
-   * Adds a rotation to the current rotation.
-   *
-   * This is similar to the + operator except that it mutates the current
-   * object.
-   *
-   * @param other The rotation to add.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Rotation2d& operator+=(const Rotation2d& other);
-
-  /**
-   * Subtracts the new rotation from the current rotation and returns the new
-   * rotation.
-   *
-   * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to subtract.
-   *
-   * @return The difference between the two rotations.
-   */
-  Rotation2d operator-(const Rotation2d& other) const;
-
-  /**
-   * Subtracts the new rotation from the current rotation.
-   *
-   * This is similar to the - operator except that it mutates the current
-   * object.
-   *
-   * @param other The rotation to subtract.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Rotation2d& operator-=(const Rotation2d& other);
-
-  /**
-   * Takes the inverse of the current rotation. This is simply the negative of
-   * the current angular value.
-   *
-   * @return The inverse of the current rotation.
-   */
-  Rotation2d operator-() const;
-
-  /**
-   * Multiplies the current rotation by a scalar.
-   * @param scalar The scalar.
-   *
-   * @return The new scaled Rotation2d.
-   */
-  Rotation2d operator*(double scalar) const;
-
-  /**
-   * Checks equality between this Rotation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Rotation2d& other) const;
-
-  /**
-   * Checks inequality between this Rotation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Rotation2d& other) const;
-
-  /**
-   * Adds the new rotation to the current rotation using a rotation matrix.
-   *
-   * [cos_new]   [other.cos, -other.sin][cos]
-   * [sin_new] = [other.sin,  other.cos][sin]
-   *
-   * value_new = std::atan2(cos_new, sin_new)
-   *
-   * @param other The rotation to rotate by.
-   *
-   * @return The new rotated Rotation2d.
-   */
-  Rotation2d RotateBy(const Rotation2d& other) const;
-
-  /**
-   * Returns the radian value of the rotation.
-   *
-   * @return The radian value of the rotation.
-   */
-  units::radian_t Radians() const { return m_value; }
-
-  /**
-   * Returns the degree value of the rotation.
-   *
-   * @return The degree value of the rotation.
-   */
-  units::degree_t Degrees() const { return m_value; }
-
-  /**
-   * Returns the cosine of the rotation.
-   *
-   * @return The cosine of the rotation.
-   */
-  double Cos() const { return m_cos; }
-
-  /**
-   * Returns the sine of the rotation.
-   *
-   * @return The sine of the rotation.
-   */
-  double Sin() const { return m_sin; }
-
-  /**
-   * Returns the tangent of the rotation.
-   *
-   * @return The tangent of the rotation.
-   */
-  double Tan() const { return m_sin / m_cos; }
-
- private:
-  units::radian_t m_value = 0_deg;
-  double m_cos = 1;
-  double m_sin = 0;
-};
-
-void to_json(wpi::json& json, const Rotation2d& rotation);
-
-void from_json(const wpi::json& json, Rotation2d& rotation);
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/geometry/Transform2d.h

diff --git a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h
deleted file mode 100644
index c75fbeb..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "Translation2d.h"
-
-namespace frc {
-
-class Pose2d;
-
-/**
- * Represents a transformation for a Pose2d.
- */
-class Transform2d {
- public:
-  /**
-   * Constructs the transform that maps the initial pose to the final pose.
-   *
-   * @param initial The initial pose for the transformation.
-   * @param final The final pose for the transformation.
-   */
-  Transform2d(Pose2d initial, Pose2d final);
-
-  /**
-   * Constructs a transform with the given translation and rotation components.
-   *
-   * @param translation Translational component of the transform.
-   * @param rotation Rotational component of the transform.
-   */
-  Transform2d(Translation2d translation, Rotation2d rotation);
-
-  /**
-   * Constructs the identity transform -- maps an initial pose to itself.
-   */
-  constexpr Transform2d() = default;
-
-  /**
-   * Returns the translation component of the transformation.
-   *
-   * @return Reference to the translational component of the transform.
-   */
-  const Translation2d& Translation() const { return m_translation; }
-
-  /**
-   * Returns the rotational component of the transformation.
-   *
-   * @return Reference to the rotational component of the transform.
-   */
-  const Rotation2d& Rotation() const { return m_rotation; }
-
-  /**
-   * Scales the transform by the scalar.
-   *
-   * @param scalar The scalar.
-   * @return The scaled Transform2d.
-   */
-  Transform2d operator*(double scalar) const {
-    return Transform2d(m_translation * scalar, m_rotation * scalar);
-  }
-
-  /**
-   * Checks equality between this Transform2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Transform2d& other) const;
-
-  /**
-   * Checks inequality between this Transform2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Transform2d& other) const;
-
- private:
-  Translation2d m_translation;
-  Rotation2d m_rotation;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/geometry/Translation2d.h

diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
deleted file mode 100644
index 90c61e2..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
+++ /dev/null
@@ -1,223 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "Rotation2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * Represents a translation in 2d space.
- * This object can be used to represent a point or a vector.
- *
- * This assumes that you are using conventional mathematical axes.
- * When the robot is placed on the origin, facing toward the X direction,
- * moving forward increases the X, whereas moving to the left increases the Y.
- */
-class Translation2d {
- public:
-  /**
-   * Constructs a Translation2d with X and Y components equal to zero.
-   */
-  constexpr Translation2d() = default;
-
-  /**
-   * Constructs a Translation2d with the X and Y components equal to the
-   * provided values.
-   *
-   * @param x The x component of the translation.
-   * @param y The y component of the translation.
-   */
-  Translation2d(units::meter_t x, units::meter_t y);
-
-  /**
-   * Calculates the distance between two translations in 2d space.
-   *
-   * This function uses the pythagorean theorem to calculate the distance.
-   * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
-   *
-   * @param other The translation to compute the distance to.
-   *
-   * @return The distance between the two translations.
-   */
-  units::meter_t Distance(const Translation2d& other) const;
-
-  /**
-   * Returns the X component of the translation.
-   *
-   * @return The x component of the translation.
-   */
-  units::meter_t X() const { return m_x; }
-
-  /**
-   * Returns the Y component of the translation.
-   *
-   * @return The y component of the translation.
-   */
-  units::meter_t Y() const { return m_y; }
-
-  /**
-   * Returns the norm, or distance from the origin to the translation.
-   *
-   * @return The norm of the translation.
-   */
-  units::meter_t Norm() const;
-
-  /**
-   * Applies a rotation to the translation in 2d space.
-   *
-   * This multiplies the translation vector by a counterclockwise rotation
-   * matrix of the given angle.
-   *
-   * [x_new]   [other.cos, -other.sin][x]
-   * [y_new] = [other.sin,  other.cos][y]
-   *
-   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
-   * Translation2d of {0, 2}.
-   *
-   * @param other The rotation to rotate the translation by.
-   *
-   * @return The new rotated translation.
-   */
-  Translation2d RotateBy(const Rotation2d& other) const;
-
-  /**
-   * Adds two translations in 2d space and returns the sum. This is similar to
-   * vector addition.
-   *
-   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
-   * Translation2d{3.0, 8.0}
-   *
-   * @param other The translation to add.
-   *
-   * @return The sum of the translations.
-   */
-  Translation2d operator+(const Translation2d& other) const;
-
-  /**
-   * Adds the new translation to the current translation.
-   *
-   * This is similar to the + operator, except that the current object is
-   * mutated.
-   *
-   * @param other The translation to add.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator+=(const Translation2d& other);
-
-  /**
-   * Subtracts the other translation from the other translation and returns the
-   * difference.
-   *
-   * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
-   * Translation2d{4.0, 2.0}
-   *
-   * @param other The translation to subtract.
-   *
-   * @return The difference between the two translations.
-   */
-  Translation2d operator-(const Translation2d& other) const;
-
-  /**
-   * Subtracts the new translation from the current translation.
-   *
-   * This is similar to the - operator, except that the current object is
-   * mutated.
-   *
-   * @param other The translation to subtract.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator-=(const Translation2d& other);
-
-  /**
-   * Returns the inverse of the current translation. This is equivalent to
-   * rotating by 180 degrees, flipping the point over both axes, or simply
-   * negating both components of the translation.
-   *
-   * @return The inverse of the current translation.
-   */
-  Translation2d operator-() const;
-
-  /**
-   * Multiplies the translation by a scalar and returns the new translation.
-   *
-   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
-   *
-   * @param scalar The scalar to multiply by.
-   *
-   * @return The scaled translation.
-   */
-  Translation2d operator*(double scalar) const;
-
-  /**
-   * Multiplies the current translation by a scalar.
-   *
-   * This is similar to the * operator, except that current object is mutated.
-   *
-   * @param scalar The scalar to multiply by.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator*=(double scalar);
-
-  /**
-   * Divides the translation by a scalar and returns the new translation.
-   *
-   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
-   *
-   * @param scalar The scalar to divide by.
-   *
-   * @return The scaled translation.
-   */
-  Translation2d operator/(double scalar) const;
-
-  /**
-   * Checks equality between this Translation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Translation2d& other) const;
-
-  /**
-   * Checks inequality between this Translation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Translation2d& other) const;
-
-  /*
-   * Divides the current translation by a scalar.
-   *
-   * This is similar to the / operator, except that current object is mutated.
-   *
-   * @param scalar The scalar to divide by.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator/=(double scalar);
-
- private:
-  units::meter_t m_x = 0_m;
-  units::meter_t m_y = 0_m;
-};
-
-void to_json(wpi::json& json, const Translation2d& state);
-
-void from_json(const wpi::json& json, Translation2d& state);
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/geometry/Twist2d.h

diff --git a/wpilibc/src/main/native/include/frc/geometry/Twist2d.h b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h
deleted file mode 100644
index ab246a0..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Twist2d.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <units/units.h>
-
-namespace frc {
-/**
- * A change in distance along arc since the last pose update. We can use ideas
- * from differential calculus to create new Pose2ds from a Twist2d and vise
- * versa.
- *
- * A Twist can be used to represent a difference between two poses.
- */
-struct Twist2d {
-  /**
-   * Linear "dx" component
-   */
-  units::meter_t dx = 0_m;
-
-  /**
-   * Linear "dy" component
-   */
-  units::meter_t dy = 0_m;
-
-  /**
-   * Angular "dtheta" component (radians)
-   */
-  units::radian_t dtheta = 0_rad;
-
-  /**
-   * Checks equality between this Twist2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Twist2d& other) const {
-    return units::math::abs(dx - other.dx) < 1E-9_m &&
-           units::math::abs(dy - other.dy) < 1E-9_m &&
-           units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
-  }
-
-  /**
-   * Checks inequality between this Twist2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Twist2d& other) const { return !operator==(other); }
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/interfaces/Gyro.h

diff --git a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
index b5aa332..8c06993 100644
--- a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
+++ b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,10 @@
 
 #pragma once
 
+#include <units/angle.h>
+
+#include "frc/geometry/Rotation2d.h"
+
 namespace frc {
 
 /**
@@ -21,12 +25,10 @@
   Gyro& operator=(Gyro&&) = default;
 
   /**
-   * Calibrate the gyro by running for a number of samples and computing the
-   * center value. Then use the center value as the Accumulator center value for
-   * subsequent measurements. It's important to make sure that the robot is not
-   * moving while the centering calculations are in progress, this is typically
+   * Calibrate the gyro. It's important to make sure that the robot is not
+   * moving while the calibration is in progress, this is typically
    * done when the robot is first turned on while it's sitting at rest before
-   * the competition starts.
+   * the match starts.
    */
   virtual void Calibrate() = 0;
 
@@ -38,17 +40,14 @@
   virtual void Reset() = 0;
 
   /**
-   * Return the actual angle in degrees that the robot is currently facing.
+   * Return the heading of the robot in degrees.
    *
-   * The angle is based on the current accumulator value corrected by the
-   * oversampling rate, the gyro type and the A/D calibration values. The angle
-   * is continuous, that is it will continue from 360 to 361 degrees. This
-   * allows algorithms that wouldn't want to see a discontinuity in the gyro
-   * output as it sweeps past from 360 to 0 on the second time around.
+   * The angle is continuous, that is it will continue from 360 to 361 degrees.
+   * This allows algorithms that wouldn't want to see a discontinuity in the
+   * gyro output as it sweeps past from 360 to 0 on the second time around.
    *
    * The angle is expected to increase as the gyro turns clockwise when looked
-   * at from the top. It needs to follow NED axis conventions in order to work
-   * properly with dependent control loops.
+   * at from the top. It needs to follow the NED axis convention.
    *
    * @return the current heading of the robot in degrees. This heading is based
    *         on integration of the returned rate from the gyro.
@@ -61,12 +60,28 @@
    * The rate is based on the most recent reading of the gyro analog value.
    *
    * The rate is expected to be positive as the gyro turns clockwise when looked
-   * at from the top. It needs to follow NED axis conventions in order to work
-   * properly with dependent control loops.
+   * at from the top. It needs to follow the NED axis convention.
    *
    * @return the current rate in degrees per second
    */
   virtual double GetRate() const = 0;
+
+  /**
+   * Return the heading of the robot as a Rotation2d.
+   *
+   * The angle is continuous, that is it will continue from 360 to 361 degrees.
+   * This allows algorithms that wouldn't want to see a discontinuity in the
+   * gyro output as it sweeps past from 360 to 0 on the second time around.
+   *
+   * The angle is expected to increase as the gyro turns counterclockwise when
+   * looked at from the top. It needs to follow the NWU axis convention.
+   *
+   * @return the current heading of the robot as a Rotation2d. This heading is
+   *         based on integration of the returned rate from the gyro.
+   */
+  virtual Rotation2d GetRotation2d() const {
+    return Rotation2d{units::degree_t{-GetAngle()}};
+  }
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h
deleted file mode 100644
index 8c772c0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/geometry/Rotation2d.h"
-
-namespace frc {
-/**
- * Represents the speed of a robot chassis. Although this struct contains
- * similar members compared to a Twist2d, they do NOT represent the same thing.
- * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
- * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
- * frame of reference.
- *
- * A strictly non-holonomic drivetrain, such as a differential drive, should
- * never have a dy component because it can never move sideways. Holonomic
- * drivetrains such as swerve and mecanum will often have all three components.
- */
-struct ChassisSpeeds {
-  /**
-   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
-   */
-  units::meters_per_second_t vx = 0_mps;
-
-  /**
-   * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
-   */
-  units::meters_per_second_t vy = 0_mps;
-
-  /**
-   * Represents the angular velocity of the robot frame. (CCW is +)
-   */
-  units::radians_per_second_t omega = 0_rad_per_s;
-
-  /**
-   * Converts a user provided field-relative set of speeds into a robot-relative
-   * ChassisSpeeds object.
-   *
-   * @param vx The component of speed in the x direction relative to the field.
-   * Positive x is away from your alliance wall.
-   * @param vy The component of speed in the y direction relative to the field.
-   * Positive y is to your left when standing behind the alliance wall.
-   * @param omega The angular rate of the robot.
-   * @param robotAngle The angle of the robot as measured by a gyroscope. The
-   * robot's angle is considered to be zero when it is facing directly away from
-   * your alliance station wall. Remember that this should be CCW positive.
-   *
-   * @return ChassisSpeeds object representing the speeds in the robot's frame
-   * of reference.
-   */
-  static ChassisSpeeds FromFieldRelativeSpeeds(
-      units::meters_per_second_t vx, units::meters_per_second_t vy,
-      units::radians_per_second_t omega, const Rotation2d& robotAngle) {
-    return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-            -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
-  }
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
deleted file mode 100644
index e986029..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <hal/FRCUsageReporting.h>
-#include <units/units.h>
-
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
-
-namespace frc {
-/**
- * Helper class that converts a chassis velocity (dx and dtheta components) to
- * left and right wheel velocities for a differential drive.
- *
- * Inverse kinematics converts a desired chassis speed into left and right
- * velocity components whereas forward kinematics converts left and right
- * component velocities into a linear and angular chassis speed.
- */
-class DifferentialDriveKinematics {
- public:
-  /**
-   * Constructs a differential drive kinematics object.
-   *
-   * @param trackWidth The track width of the drivetrain. Theoretically, this is
-   * the distance between the left wheels and right wheels. However, the
-   * empirical value may be larger than the physical measured value due to
-   * scrubbing effects.
-   */
-  explicit DifferentialDriveKinematics(units::meter_t trackWidth)
-      : trackWidth(trackWidth) {
-    HAL_Report(HALUsageReporting::kResourceType_Kinematics,
-               HALUsageReporting::kKinematics_DifferentialDrive);
-  }
-
-  /**
-   * Returns a chassis speed from left and right component velocities using
-   * forward kinematics.
-   *
-   * @param wheelSpeeds The left and right velocities.
-   * @return The chassis speed.
-   */
-  constexpr ChassisSpeeds ToChassisSpeeds(
-      const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
-    return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
-            (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
-  }
-
-  /**
-   * Returns left and right component velocities from a chassis speed using
-   * inverse kinematics.
-   *
-   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
-   * represent the chassis' speed.
-   * @return The left and right velocities.
-   */
-  constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds) const {
-    return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
-            chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
-  }
-
-  units::meter_t trackWidth;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
deleted file mode 100644
index 379839d..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ /dev/null
@@ -1,89 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "DifferentialDriveKinematics.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc2/Timer.h"
-
-namespace frc {
-/**
- * Class for differential drive odometry. Odometry allows you to track the
- * robot's position on the field over the course of a match using readings from
- * 2 encoders and a gyroscope.
- *
- * Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- *
- * It is important that you reset your encoders to zero before using this class.
- * Any subsequent pose resets also require the encoders to be reset to zero.
- */
-class DifferentialDriveOdometry {
- public:
-  /**
-   * Constructs a DifferentialDriveOdometry object.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param initialPose The starting position of the robot on the field.
-   */
-  explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
-                                     const Pose2d& initialPose = Pose2d());
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * You NEED to reset your encoders (to zero) when calling this method.
-   *
-   * The gyroscope angle does not need to be reset here on the user's robot
-   * code. The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param pose The position on the field that your robot is at.
-   * @param gyroAngle The angle reported by the gyroscope.
-   */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-    m_prevLeftDistance = 0_m;
-    m_prevRightDistance = 0_m;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
-   * Updates the robot position on the field using distance measurements from
-   * encoders. This method is more numerically accurate than using velocities to
-   * integrate the pose and is also advantageous for teams that are using lower
-   * CPR encoders.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param leftDistance The distance traveled by the left encoder.
-   * @param rightDistance The distance traveled by the right encoder.
-   * @return The new pose of the robot.
-   */
-  const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                       units::meter_t rightDistance);
-
- private:
-  Pose2d m_pose;
-
-  Rotation2d m_gyroOffset;
-  Rotation2d m_previousAngle;
-
-  units::meter_t m_prevLeftDistance = 0_m;
-  units::meter_t m_prevRightDistance = 0_m;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
deleted file mode 100644
index 66bd84e..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-namespace frc {
-/**
- * Represents the wheel speeds for a differential drive drivetrain.
- */
-struct DifferentialDriveWheelSpeeds {
-  /**
-   * Speed of the left side of the robot.
-   */
-  units::meters_per_second_t left = 0_mps;
-
-  /**
-   * Speed of the right side of the robot.
-   */
-  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.
-   *
-   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
-   */
-  void Normalize(units::meters_per_second_t attainableMaxSpeed);
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
deleted file mode 100644
index 47b1b34..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ /dev/null
@@ -1,144 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <Eigen/Core>
-#include <Eigen/QR>
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
-
-namespace frc {
-
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual wheel speeds.
- *
- * The inverse kinematics (converting from a desired chassis velocity to
- * individual wheel speeds) uses the relative locations of the wheels with
- * respect to the center of rotation. The center of rotation for inverse
- * kinematics is also variable. This means that you can set your set your center
- * of rotation in a corner of the robot to perform special evasion manuevers.
- *
- * Forward kinematics (converting an array of wheel speeds into the overall
- * chassis motion) is performs the exact opposite of what inverse kinematics
- * does. Since this is an overdetermined system (more equations than variables),
- * we use a least-squares approximation.
- *
- * The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
- * multiply by [wheelSpeeds] to get our chassis speeds.
- *
- * Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-class MecanumDriveKinematics {
- public:
-  /**
-   * Constructs a mecanum drive kinematics object.
-   *
-   * @param frontLeftWheel The location of the front-left wheel relative to the
-   *                       physical center of the robot.
-   * @param frontRightWheel The location of the front-right wheel relative to
-   *                        the physical center of the robot.
-   * @param rearLeftWheel The location of the rear-left wheel relative to the
-   *                      physical center of the robot.
-   * @param rearRightWheel The location of the rear-right wheel relative to the
-   *                       physical center of the robot.
-   */
-  explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
-                                  Translation2d frontRightWheel,
-                                  Translation2d rearLeftWheel,
-                                  Translation2d rearRightWheel)
-      : m_frontLeftWheel{frontLeftWheel},
-        m_frontRightWheel{frontRightWheel},
-        m_rearLeftWheel{rearLeftWheel},
-        m_rearRightWheel{rearRightWheel} {
-    SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
-                         rearRightWheel);
-    m_forwardKinematics = m_inverseKinematics.householderQr();
-    HAL_Report(HALUsageReporting::kResourceType_Kinematics,
-               HALUsageReporting::kKinematics_MecanumDrive);
-  }
-
-  MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
-
-  /**
-   * Performs inverse kinematics to return the wheel speeds from a desired
-   * chassis velocity. This method is often used to convert joystick values into
-   * wheel speeds.
-   *
-   * This function also supports variable centers of rotation. During normal
-   * operations, the center of rotation is usually the same as the physical
-   * center of the robot; therefore, the argument is defaulted to that use case.
-   * However, if you wish to change the center of rotation for evasive
-   * manuevers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @param centerOfRotation The center of rotation. For example, if you set the
-   *                         center of rotation at one corner of the robot and
-   *                         provide a chassis speed that only has a dtheta
-   *                         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
-   *         MecanumDriveWheelSpeeds::Normalize() function to rectify this
-   *         issue. In addition, you can leverage the power of C++17 to directly
-   *         assign the wheel speeds to variables:
-   *
-   * @code{.cpp}
-   * auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
-   * @endcode
-   */
-  MecanumDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d());
-
-  /**
-   * Performs forward kinematics to return the resulting chassis state from the
-   * given wheel speeds. This method is often used for odometry -- determining
-   * the robot's position on the field using data from the real-world speed of
-   * each wheel on the robot.
-   *
-   * @param wheelSpeeds The current mecanum drive wheel speeds.
-   *
-   * @return The resulting chassis speed.
-   */
-  ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds& wheelSpeeds);
-
- private:
-  Eigen::Matrix<double, 4, 3> m_inverseKinematics;
-  Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
-  Translation2d m_frontLeftWheel;
-  Translation2d m_frontRightWheel;
-  Translation2d m_rearLeftWheel;
-  Translation2d m_rearRightWheel;
-
-  Translation2d m_previousCoR;
-
-  /**
-   * Construct inverse kinematics matrix from wheel locations.
-   *
-   * @param fl The location of the front-left wheel relative to the physical
-   *           center of the robot.
-   * @param fr The location of the front-right wheel relative to the physical
-   *           center of the robot.
-   * @param rl The location of the rear-left wheel relative to the physical
-   *           center of the robot.
-   * @param rr The location of the rear-right wheel relative to the physical
-   *           center of the robot.
-   */
-  void SetInverseKinematics(Translation2d fl, Translation2d fr,
-                            Translation2d rl, Translation2d rr);
-};
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
deleted file mode 100644
index 697a6b0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
-#include "frc2/Timer.h"
-
-namespace frc {
-
-/**
- * Class for mecanum drive odometry. Odometry allows you to track the robot's
- * position on the field over a course of a match using readings from your
- * mecanum wheel encoders.
- *
- * Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- */
-class MecanumDriveOdometry {
- public:
-  /**
-   * Constructs a MecanumDriveOdometry object.
-   *
-   * @param kinematics The mecanum drive kinematics for your drivetrain.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param initialPose The starting position of the robot on the field.
-   */
-  explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
-                                const Rotation2d& gyroAngle,
-                                const Pose2d& initialPose = Pose2d());
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset here on the user's robot
-   * code. The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param pose The position on the field that your robot is at.
-   * @param gyroAngle The angle reported by the gyroscope.
-   */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param currentTime The current time.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               MecanumDriveWheelSpeeds wheelSpeeds);
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates
-   * the current time to calculate period (difference between two timestamps).
-   * The period is used to calculate the change in distance from a velocity.
-   * This also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
-                          wheelSpeeds);
-  }
-
- private:
-  MecanumDriveKinematics m_kinematics;
-  Pose2d m_pose;
-
-  units::second_t m_previousTime = -1_s;
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
-};
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
deleted file mode 100644
index 159f7f0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-namespace frc {
-/**
- * Represents the wheel speeds for a mecanum drive drivetrain.
- */
-struct MecanumDriveWheelSpeeds {
-  /**
-   * Speed of the front-left wheel.
-   */
-  units::meters_per_second_t frontLeft = 0_mps;
-
-  /**
-   * Speed of the front-right wheel.
-   */
-  units::meters_per_second_t frontRight = 0_mps;
-
-  /**
-   * Speed of the rear-left wheel.
-   */
-  units::meters_per_second_t rearLeft = 0_mps;
-
-  /**
-   * Speed of the rear-right wheel.
-   */
-  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.
-   *
-   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
-   */
-  void Normalize(units::meters_per_second_t attainableMaxSpeed);
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
deleted file mode 100644
index f889363..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ /dev/null
@@ -1,170 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <array>
-#include <cstddef>
-
-#include <Eigen/Core>
-#include <Eigen/QR>
-#include <hal/FRCUsageReporting.h>
-#include <units/units.h>
-
-#include "frc/geometry/Rotation2d.h"
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/SwerveModuleState.h"
-
-namespace frc {
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual module states (speed and angle).
- *
- * The inverse kinematics (converting from a desired chassis velocity to
- * individual module states) uses the relative locations of the modules with
- * respect to the center of rotation. The center of rotation for inverse
- * kinematics is also variable. This means that you can set your set your center
- * of rotation in a corner of the robot to perform special evasion manuevers.
- *
- * Forward kinematics (converting an array of module states into the overall
- * chassis motion) is performs the exact opposite of what inverse kinematics
- * does. Since this is an overdetermined system (more equations than variables),
- * we use a least-squares approximation.
- *
- * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
- * multiply by [moduleStates] to get our chassis speeds.
- *
- * Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-template <size_t NumModules>
-class SwerveDriveKinematics {
- public:
-  /**
-   * Constructs a swerve drive kinematics object. This takes in a variable
-   * number of wheel locations as Translation2ds. The order in which you pass in
-   * the wheel locations is the same order that you will recieve the module
-   * states when performing inverse kinematics. It is also expected that you
-   * pass in the module states in the same order when calling the forward
-   * kinematics methods.
-   *
-   * @param wheels The locations of the wheels relative to the physical center
-   * of the robot.
-   */
-  template <typename... Wheels>
-  explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
-      : m_modules{wheel, wheels...} {
-    static_assert(sizeof...(wheels) >= 1,
-                  "A swerve drive requires at least two modules");
-
-    for (size_t i = 0; i < NumModules; i++) {
-      // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
-        1, 0, (-m_modules[i].Y()).template to<double>(),
-        0, 1, (+m_modules[i].X()).template to<double>();
-      // clang-format on
-    }
-
-    m_forwardKinematics = m_inverseKinematics.householderQr();
-
-    HAL_Report(HALUsageReporting::kResourceType_Kinematics,
-               HALUsageReporting::kKinematics_SwerveDrive);
-  }
-
-  SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
-
-  /**
-   * Performs inverse kinematics to return the module states from a desired
-   * chassis velocity. This method is often used to convert joystick values into
-   * module speeds and angles.
-   *
-   * This function also supports variable centers of rotation. During normal
-   * operations, the center of rotation is usually the same as the physical
-   * center of the robot; therefore, the argument is defaulted to that use case.
-   * However, if you wish to change the center of rotation for evasive
-   * manuevers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @param centerOfRotation The center of rotation. For example, if you set the
-   * center of rotation at one corner of the robot and provide a chassis speed
-   * that only has a dtheta 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
-   * <NormalizeWheelSpeeds> function to rectify this issue. In addition, you can
-   * leverage the power of C++17 to directly assign the module states to
-   * variables:
-   *
-   * @code{.cpp}
-   * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
-   * @endcode
-   */
-  std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
-      const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d());
-
-  /**
-   * Performs forward kinematics to return the resulting chassis state from the
-   * given module states. This method is often used for odometry -- determining
-   * the robot's position on the field using data from the real-world speed and
-   * angle of each module on the robot.
-   *
-   * @param wheelStates The state of the modules (as a SwerveModuleState type)
-   * as measured from respective encoders and gyros. The order of the swerve
-   * module states should be same as passed into the constructor of this class.
-   *
-   * @return The resulting chassis speed.
-   */
-  template <typename... ModuleStates>
-  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
-
-  /**
-   * Performs forward kinematics to return the resulting chassis state from the
-   * given module states. This method is often used for odometry -- determining
-   * the robot's position on the field using data from the real-world speed and
-   * angle of each module on the robot.
-   *
-   * @param moduleStates The state of the modules as an std::array of type
-   * SwerveModuleState, NumModules long as measured from respective encoders
-   * and gyros. The order of the swerve module states should be same as passed
-   * into the constructor of this class.
-   *
-   * @return The resulting chassis speed.
-   */
-  ChassisSpeeds ToChassisSpeeds(
-      std::array<SwerveModuleState, NumModules> moduleStates);
-
-  /**
-   * 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.
-   *
-   * @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(
-      std::array<SwerveModuleState, NumModules>* moduleStates,
-      units::meters_per_second_t attainableMaxSpeed);
-
- private:
-  Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
-  Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
-      m_forwardKinematics;
-  std::array<Translation2d, NumModules> m_modules;
-
-  Translation2d m_previousCoR;
-};
-}  // namespace frc
-
-#include "SwerveDriveKinematics.inc"

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc

diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
deleted file mode 100644
index b362aa0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ /dev/null
@@ -1,110 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <algorithm>
-
-namespace frc {
-
-template <class... Wheels>
-SwerveDriveKinematics(Translation2d, Wheels...)
-    ->SwerveDriveKinematics<1 + sizeof...(Wheels)>;
-
-template <size_t NumModules>
-std::array<SwerveModuleState, NumModules>
-SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
-    const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
-  // We have a new center of rotation. We need to compute the matrix again.
-  if (centerOfRotation != m_previousCoR) {
-    for (size_t i = 0; i < NumModules; i++) {
-      // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
-        1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>(),
-        0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>();
-      // clang-format on
-    }
-    m_previousCoR = centerOfRotation;
-  }
-
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
-
-  Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
-      m_inverseKinematics * chassisSpeedsVector;
-  std::array<SwerveModuleState, NumModules> moduleStates;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    units::meters_per_second_t x =
-        units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)};
-    units::meters_per_second_t y =
-        units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)};
-
-    auto speed = units::math::hypot(x, y);
-    Rotation2d rotation{x.to<double>(), y.to<double>()};
-
-    moduleStates[i] = {speed, rotation};
-  }
-
-  return moduleStates;
-}
-
-template <size_t NumModules>
-template <typename... ModuleStates>
-ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    ModuleStates&&... wheelStates) {
-  static_assert(sizeof...(wheelStates) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "locations provided in constructor.");
-
-  std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
-
-  return this->ToChassisSpeeds(moduleStates);
-}
-
-template <size_t NumModules>
-ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    std::array<SwerveModuleState, NumModules> moduleStates) {
-  Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    SwerveModuleState module = moduleStates[i];
-    moduleStatesMatrix.row(i * 2)
-        << module.speed.to<double>() * module.angle.Cos();
-    moduleStatesMatrix.row(i * 2 + 1)
-        << module.speed.to<double>() * module.angle.Sin();
-  }
-
-  Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(moduleStatesMatrix);
-
-  return {units::meters_per_second_t{chassisSpeedsVector(0)},
-          units::meters_per_second_t{chassisSpeedsVector(1)},
-          units::radians_per_second_t{chassisSpeedsVector(2)}};
-}
-
-template <size_t NumModules>
-void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
-    std::array<SwerveModuleState, NumModules>* moduleStates,
-    units::meters_per_second_t attainableMaxSpeed) {
-  auto& states = *moduleStates;
-  auto realMaxSpeed = std::max_element(states.begin(), states.end(),
-                                       [](const auto& a, const auto& b) {
-                                         return units::math::abs(a.speed) <
-                                                units::math::abs(b.speed);
-                                       })
-                          ->speed;
-
-  if (realMaxSpeed > attainableMaxSpeed) {
-    for (auto& module : states) {
-      module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
-    }
-  }
-}
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
deleted file mode 100644
index 8093ca5..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <chrono>
-#include <cstddef>
-#include <ctime>
-
-#include <units/units.h>
-
-#include "SwerveDriveKinematics.h"
-#include "SwerveModuleState.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc2/Timer.h"
-
-namespace frc {
-
-/**
- * Class for swerve drive odometry. Odometry allows you to track the robot's
- * position on the field over a course of a match using readings from your
- * swerve drive encoders and swerve azimuth encoders.
- *
- * Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- */
-template <size_t NumModules>
-class SwerveDriveOdometry {
- public:
-  /**
-   * Constructs a SwerveDriveOdometry object.
-   *
-   * @param kinematics The swerve drive kinematics for your drivetrain.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param initialPose The starting position of the robot on the field.
-   */
-  SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
-                      const Rotation2d& gyroAngle,
-                      const Pose2d& initialPose = Pose2d());
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset here on the user's robot
-   * code. The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param pose The position on the field that your robot is at.
-   * @param gyroAngle The angle reported by the gyroscope.
-   */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param currentTime The current time.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide
-   *                     the states in the same order in which you instantiated
-   *                     your SwerveDriveKinematics.
-   *
-   * @return The new pose of the robot.
-   */
-  template <typename... ModuleStates>
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               ModuleStates&&... moduleStates);
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates
-   * the current time to calculate period (difference between two timestamps).
-   * The period is used to calculate the change in distance from a velocity.
-   * This also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide
-   *                     the states in the same order in which you instantiated
-   *                     your SwerveDriveKinematics.
-   *
-   * @return The new pose of the robot.
-   */
-  template <typename... ModuleStates>
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       ModuleStates&&... moduleStates) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
-                          moduleStates...);
-  }
-
- private:
-  SwerveDriveKinematics<NumModules> m_kinematics;
-  Pose2d m_pose;
-
-  units::second_t m_previousTime = -1_s;
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
-};
-
-}  // namespace frc
-
-#include "SwerveDriveOdometry.inc"

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc

diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
deleted file mode 100644
index e794388..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <hal/FRCUsageReporting.h>
-
-namespace frc {
-template <size_t NumModules>
-SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
-    SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
-    const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_SwerveDrive);
-}
-
-template <size_t NumModules>
-template <typename... ModuleStates>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    ModuleStates&&... moduleStates) {
-  units::second_t deltaTime =
-      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
-  m_previousTime = currentTime;
-
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
-  static_cast<void>(dtheta);
-
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
-}  // namespace frc

wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h

diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h
deleted file mode 100644
index fbfe0d1..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/geometry/Rotation2d.h"
-
-namespace frc {
-/**
- * Represents the state of one swerve module.
- */
-struct SwerveModuleState {
-  /**
-   * Speed of the wheel of the module.
-   */
-  units::meters_per_second_t speed = 0_mps;
-
-  /**
-   * Angle of the module.
-   */
-  Rotation2d angle;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h

diff --git a/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h b/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
index c7e0ec2..b755f46 100644
--- a/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
+++ b/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2012-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 
 namespace frc {
@@ -22,6 +23,9 @@
   LiveWindow(const LiveWindow&) = delete;
   LiveWindow& operator=(const LiveWindow&) = delete;
 
+  std::function<void()> enabled;
+  std::function<void()> disabled;
+
   /**
    * Get an instance of the LiveWindow main class.
    *

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

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

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

diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
index ccbc802..57e4743 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <wpi/StringRef.h>
+
 namespace frc {
 
 // Maintainer note: this enum is mirrored in WPILibJ and in Shuffleboard

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

diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
index b202160..2be9859 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,6 +20,9 @@
   explicit ShuffleboardInstance(nt::NetworkTableInstance ntInstance);
   virtual ~ShuffleboardInstance();
 
+  ShuffleboardInstance(ShuffleboardInstance&&) = default;
+  ShuffleboardInstance& operator=(ShuffleboardInstance&&) = default;
+
   frc::ShuffleboardTab& GetTab(wpi::StringRef title) override;
 
   void Update() override;

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

diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
index ea92b93..729da40 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -62,7 +62,7 @@
    * Sets the type of widget used to display the data. If not set, the default
    * widget type will be used. This method should only be used to use a widget
    * that does not come built into Shuffleboard (i.e. one that comes with a
-   * custom or thrid-party plugin). To use a widget that is built into
+   * custom or third-party plugin). To use a widget that is built into
    * Shuffleboard, use {@link #withWidget(WidgetType)} and {@link
    * BuiltInWidgets}.
    *

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h b/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
new file mode 100644
index 0000000..1d2d2c3
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class ADXRS450_Gyro;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class ADXRS450_GyroSim {
+ public:
+  /**
+   * Constructs from a ADXRS450_Gyro object.
+   *
+   * @param gyro ADXRS450_Gyro to simulate
+   */
+  explicit ADXRS450_GyroSim(const ADXRS450_Gyro& gyro);
+
+  /**
+   * Sets the angle.
+   *
+   * @param angle The angle (clockwise positive).
+   */
+  void SetAngle(units::degree_t angle);
+
+  /**
+   * Sets the angular rate (clockwise positive).
+   *
+   * @param rate The angular rate.
+   */
+  void SetRate(units::degrees_per_second_t rate);
+
+ private:
+  hal::SimDouble m_simAngle;
+  hal::SimDouble m_simRate;
+};
+
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h b/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h
new file mode 100644
index 0000000..0c578f4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+struct HAL_AddressableLEDData;
+
+namespace frc {
+
+class AddressableLED;
+
+namespace sim {
+
+/**
+ * Class to control a simulated addressable LED.
+ */
+class AddressableLEDSim {
+ public:
+  /**
+   * Constructs for the first addressable LED.
+   */
+  AddressableLEDSim();
+
+  /**
+   * Constructs from an AddressableLED object.
+   *
+   * @param addressableLED AddressableLED to simulate
+   */
+  explicit AddressableLEDSim(const AddressableLED& addressableLED);
+
+  /**
+   * Creates an AddressableLEDSim for a PWM channel.
+   *
+   * @param pwmChannel PWM channel
+   * @return Simulated object
+   * @throws std::out_of_range if no AddressableLED is configured for that
+   *         channel
+   */
+  static AddressableLEDSim CreateForChannel(int pwmChannel);
+
+  /**
+   * Creates an AddressableLEDSim for a simulated index.
+   * The index is incremented for each simulated AddressableLED.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static AddressableLEDSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterOutputPortCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetOutputPort() const;
+
+  void SetOutputPort(int outputPort);
+
+  std::unique_ptr<CallbackStore> RegisterLengthCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  int GetLength() const;
+
+  void SetLength(int length);
+
+  std::unique_ptr<CallbackStore> RegisterRunningCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetRunning() const;
+
+  void SetRunning(bool running);
+
+  std::unique_ptr<CallbackStore> RegisterDataCallback(NotifyCallback callback,
+                                                      bool initialNotify);
+
+  int GetData(struct HAL_AddressableLEDData* data) const;
+
+  void SetData(struct HAL_AddressableLEDData* data, int length);
+
+ private:
+  explicit AddressableLEDSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
new file mode 100644
index 0000000..aef979a
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/angle.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class AnalogEncoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog encoder.
+ */
+class AnalogEncoderSim {
+ public:
+  /**
+   * Constructs from an AnalogEncoder object.
+   *
+   * @param encoder AnalogEncoder to simulate
+   */
+  explicit AnalogEncoderSim(const AnalogEncoder& encoder);
+
+  /**
+   * Set the position using an {@link Rotation2d}.
+   *
+   * @param angle The angle.
+   */
+  void SetPosition(Rotation2d angle);
+
+  /**
+   * Set the position of the encoder.
+   *
+   * @param turns The position.
+   */
+  void SetTurns(units::turn_t turns);
+
+  /**
+   * Get the simulated position.
+   */
+  units::turn_t GetTurns();
+
+  /**
+   * Get the position as a {@link Rotation2d}.
+   */
+  Rotation2d GetPosition();
+
+ private:
+  hal::SimDouble m_positionSim;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h
new file mode 100644
index 0000000..685e3ed
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogGyro;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog gyro.
+ */
+class AnalogGyroSim {
+ public:
+  /**
+   * Constructs from an AnalogGyro object.
+   *
+   * @param gyro AnalogGyro to simulate
+   */
+  explicit AnalogGyroSim(const AnalogGyro& gyro);
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit AnalogGyroSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterAngleCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  double GetAngle() const;
+
+  void SetAngle(double angle);
+
+  std::unique_ptr<CallbackStore> RegisterRateCallback(NotifyCallback callback,
+                                                      bool initialNotify);
+
+  double GetRate() const;
+
+  void SetRate(double rate);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h
new file mode 100644
index 0000000..a52cec4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogInput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog input.
+ */
+class AnalogInputSim {
+ public:
+  /**
+   * Constructs from an AnalogInput object.
+   *
+   * @param analogInput AnalogInput to simulate
+   */
+  explicit AnalogInputSim(const AnalogInput& analogInput);
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit AnalogInputSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterAverageBitsCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetAverageBits() const;
+
+  void SetAverageBits(int averageBits);
+
+  std::unique_ptr<CallbackStore> RegisterOversampleBitsCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetOversampleBits() const;
+
+  void SetOversampleBits(int oversampleBits);
+
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetVoltage() const;
+
+  void SetVoltage(double voltage);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetAccumulatorInitialized() const;
+
+  void SetAccumulatorInitialized(bool accumulatorInitialized);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorValueCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int64_t GetAccumulatorValue() const;
+
+  void SetAccumulatorValue(int64_t accumulatorValue);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorCountCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int64_t GetAccumulatorCount() const;
+
+  void SetAccumulatorCount(int64_t accumulatorCount);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorCenterCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetAccumulatorCenter() const;
+
+  void SetAccumulatorCenter(int accumulatorCenter);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorDeadbandCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetAccumulatorDeadband() const;
+
+  void SetAccumulatorDeadband(int accumulatorDeadband);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h
new file mode 100644
index 0000000..e468fa5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog output.
+ */
+class AnalogOutputSim {
+ public:
+  /**
+   * Constructs from an AnalogOutput object.
+   *
+   * @param analogOutput AnalogOutput to simulate
+   */
+  explicit AnalogOutputSim(const AnalogOutput& analogOutput);
+
+  /**
+   * Constructs from an analog output channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit AnalogOutputSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetVoltage() const;
+
+  void SetVoltage(double voltage);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h
new file mode 100644
index 0000000..60d62e1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogTrigger;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog trigger.
+ */
+class AnalogTriggerSim {
+ public:
+  /**
+   * Constructs from an AnalogTrigger object.
+   *
+   * @param analogTrigger AnalogTrigger to simulate
+   */
+  explicit AnalogTriggerSim(const AnalogTrigger& analogTrigger);
+
+  /**
+   * Creates an AnalogTriggerSim for an analog input channel.
+   *
+   * @param channel analog input channel
+   * @return Simulated object
+   * @throws std::out_of_range if no AnalogTrigger is configured for that
+   *         channel
+   */
+  static AnalogTriggerSim CreateForChannel(int channel);
+
+  /**
+   * Creates an AnalogTriggerSim for a simulated index.
+   * The index is incremented for each simulated AnalogTrigger.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static AnalogTriggerSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterTriggerLowerBoundCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetTriggerLowerBound() const;
+
+  void SetTriggerLowerBound(double triggerLowerBound);
+
+  std::unique_ptr<CallbackStore> RegisterTriggerUpperBoundCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetTriggerUpperBound() const;
+
+  void SetTriggerUpperBound(double triggerUpperBound);
+
+  void ResetData();
+
+ private:
+  explicit AnalogTriggerSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/BatterySim.h b/wpilibc/src/main/native/include/frc/simulation/BatterySim.h
new file mode 100644
index 0000000..ef8fb5d
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/BatterySim.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <numeric>
+
+#include <units/current.h>
+#include <units/impedance.h>
+#include <units/voltage.h>
+
+namespace frc::sim {
+
+class BatterySim {
+ public:
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link
+   * RobotController#getBatteryVoltage()} method.
+   *
+   * @param nominalVoltage The nominal battery voltage. Usually 12v.
+   * @param resistance     The forward resistance of the battery. Most batteries
+   *                       are at or below 20 milliohms.
+   * @param currents       The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  static units::volt_t Calculate(
+      units::volt_t nominalVoltage, units::ohm_t resistance,
+      std::initializer_list<units::ampere_t> currents) {
+    return nominalVoltage -
+           std::accumulate(currents.begin(), currents.end(), 0_A) * resistance;
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link
+   * RobotController#getBatteryVoltage()} method. This function assumes a
+   * nominal voltage of 12v and a resistance of 20 milliohms (0.020 ohms)
+   *
+   * @param currents The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  static units::volt_t Calculate(
+      std::initializer_list<units::ampere_t> currents) {
+    return Calculate(12_V, 0.02_Ohm, currents);
+  }
+};
+
+}  // namespace frc::sim

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h b/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h
new file mode 100644
index 0000000..49a581c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Accelerometer.h>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class BuiltInAccelerometer;
+
+namespace sim {
+
+/**
+ * Class to control a simulated built-in accelerometer.
+ */
+class BuiltInAccelerometerSim {
+ public:
+  /**
+   * Constructs for the first built-in accelerometer.
+   */
+  BuiltInAccelerometerSim();
+
+  /**
+   * Constructs from a BuiltInAccelerometer object.
+   *
+   * @param accel BuiltInAccelerometer to simulate
+   */
+  explicit BuiltInAccelerometerSim(const BuiltInAccelerometer& accel);
+
+  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  bool GetActive() const;
+
+  void SetActive(bool active);
+
+  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  HAL_AccelerometerRange GetRange() const;
+
+  void SetRange(HAL_AccelerometerRange range);
+
+  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetX() const;
+
+  void SetX(double x);
+
+  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetY() const;
+
+  void SetY(double y);
+
+  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetZ() const;
+
+  void SetZ(double z);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h b/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h
new file mode 100644
index 0000000..379296a
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+
+#include <hal/Value.h>
+#include <wpi/StringRef.h>
+
+namespace frc {
+namespace sim {
+
+using NotifyCallback = std::function<void(wpi::StringRef, const HAL_Value*)>;
+using ConstBufferCallback = std::function<void(
+    wpi::StringRef, const unsigned char* buffer, unsigned int count)>;
+typedef void (*CancelCallbackFunc)(int32_t index, int32_t uid);
+typedef void (*CancelCallbackNoIndexFunc)(int32_t uid);
+typedef void (*CancelCallbackChannelFunc)(int32_t index, int32_t channel,
+                                          int32_t uid);
+
+void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
+void ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                   const unsigned char* buffer,
+                                   unsigned int count);
+
+class CallbackStore {
+ public:
+  CallbackStore(int32_t i, NotifyCallback cb, CancelCallbackNoIndexFunc ccf);
+
+  CallbackStore(int32_t i, int32_t u, NotifyCallback cb,
+                CancelCallbackFunc ccf);
+
+  CallbackStore(int32_t i, int32_t c, int32_t u, NotifyCallback cb,
+                CancelCallbackChannelFunc ccf);
+
+  CallbackStore(int32_t i, ConstBufferCallback cb,
+                CancelCallbackNoIndexFunc ccf);
+
+  CallbackStore(int32_t i, int32_t u, ConstBufferCallback cb,
+                CancelCallbackFunc ccf);
+
+  CallbackStore(int32_t i, int32_t c, int32_t u, ConstBufferCallback cb,
+                CancelCallbackChannelFunc ccf);
+
+  CallbackStore(const CallbackStore&) = delete;
+  CallbackStore& operator=(const CallbackStore&) = delete;
+
+  ~CallbackStore();
+
+  void SetUid(int32_t uid);
+
+  friend void CallbackStoreThunk(const char* name, void* param,
+                                 const HAL_Value* value);
+
+  friend void ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                            const unsigned char* buffer,
+                                            unsigned int count);
+
+ private:
+  int32_t index;
+  int32_t channel;
+  int32_t uid;
+
+  NotifyCallback callback;
+  ConstBufferCallback constBufferCallback;
+  union {
+    CancelCallbackFunc ccf;
+    CancelCallbackChannelFunc cccf;
+    CancelCallbackNoIndexFunc ccnif;
+  };
+  enum CancelType { Normal, Channel, NoIndex };
+  CancelType cancelType;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/DIOSim.h b/wpilibc/src/main/native/include/frc/simulation/DIOSim.h
new file mode 100644
index 0000000..f6edb39
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DIOSim.h
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DigitalInput;
+class DigitalOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated digital input or output.
+ */
+class DIOSim {
+ public:
+  /**
+   * Constructs from a DigitalInput object.
+   *
+   * @param input DigitalInput to simulate
+   */
+  explicit DIOSim(const DigitalInput& input);
+
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param output DigitalOutput to simulate
+   */
+  explicit DIOSim(const DigitalOutput& output);
+
+  /**
+   * Constructs from an digital I/O channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit DIOSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterValueCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  bool GetValue() const;
+
+  void SetValue(bool value);
+
+  std::unique_ptr<CallbackStore> RegisterPulseLengthCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetPulseLength() const;
+
+  void SetPulseLength(double pulseLength);
+
+  std::unique_ptr<CallbackStore> RegisterIsInputCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetIsInput() const;
+
+  void SetIsInput(bool isInput);
+
+  std::unique_ptr<CallbackStore> RegisterFilterIndexCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetFilterIndex() const;
+
+  void SetFilterIndex(int filterIndex);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
new file mode 100644
index 0000000..a75edb9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -0,0 +1,235 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+
+#include <Eigen/Core>
+#include <units/length.h>
+#include <units/moment_of_inertia.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace frc::sim {
+
+class DifferentialDrivetrainSim {
+ public:
+  /**
+   * Create a SimDrivetrain.
+   *
+   * @param drivetrainPlant   The LinearSystem representing the robot's
+   * drivetrain. This system can be created with
+   * LinearSystemId#createDrivetrainVelocitySystem or
+   * LinearSystemId#identifyDrivetrainSystem.
+   * @param trackWidth        The robot's track width.
+   * @param driveMotor        A {@link DCMotor} representing the left side of
+   * the drivetrain.
+   * @param gearingRatio      The gearingRatio ratio of the left side, as output
+   * over input. This must be the same ratio as the ratio used to identify or
+   * create the drivetrainPlant.
+   * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in
+   * meters.
+   */
+  DifferentialDrivetrainSim(const LinearSystem<2, 2, 2>& plant,
+                            units::meter_t trackWidth, DCMotor driveMotor,
+                            double gearingRatio, units::meter_t wheelRadius);
+
+  /**
+   * Create a SimDrivetrain.
+   *
+   * @param driveMotor  A {@link DCMotor} representing the left side of the
+   * drivetrain.
+   * @param gearing     The gearing on the drive between motor and wheel, as
+   * output over input. This must be the same ratio as the ratio used to
+   * identify or create the drivetrainPlant.
+   * @param J           The moment of inertia of the drivetrain about its
+   * center.
+   * @param mass        The mass of the drivebase.
+   * @param wheelRadius The radius of the wheels on the drivetrain.
+   * @param trackWidth  The robot's track width, or distance between left and
+   * right wheels.
+   */
+  DifferentialDrivetrainSim(frc::DCMotor driveMotor, double gearing,
+                            units::kilogram_square_meter_t J,
+                            units::kilogram_t mass, units::meter_t wheelRadius,
+                            units::meter_t trackWidth);
+
+  /**
+   * Sets the applied voltage to the drivetrain. Note that positive voltage must
+   * make that side of the drivetrain travel forward (+X).
+   *
+   * @param leftVoltage  The left voltage.
+   * @param rightVoltage The right voltage.
+   */
+  void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage);
+
+  /**
+   * Sets the gearing reduction on the drivetrain. This is commonly used for
+   * shifting drivetrains.
+   *
+   * @param newGearing The new gear ratio, as output over input.
+   */
+  void SetGearing(double newGearing);
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dt The time that's passed since the last {@link #update(double)}
+   * call.
+   */
+  void Update(units::second_t dt);
+
+  /**
+   * Returns an element of the state vector.
+   *
+   * @param state The row of the state vector.
+   */
+  double GetState(int state) const;
+
+  /**
+   * Returns the current gearing reduction of the drivetrain, as output over
+   * input.
+   */
+  double GetGearing() const;
+
+  /**
+   * Returns the current state vector x.
+   */
+  Eigen::Matrix<double, 7, 1> GetState() const;
+
+  /**
+   * Returns the direction the robot is pointing.
+   *
+   * Note that this angle is counterclockwise-positive, while most gyros are
+   * clockwise positive.
+   */
+  Rotation2d GetHeading() const;
+
+  /**
+   * Returns the current pose.
+   */
+  Pose2d GetPose() const;
+
+  /**
+   * Returns the currently drawn current.
+   */
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The state.
+   */
+  void SetState(const Eigen::Matrix<double, 7, 1>& state);
+
+  /**
+   * Sets the system pose.
+   *
+   * @param pose The pose.
+   */
+  void SetPose(const frc::Pose2d& pose);
+
+  Eigen::Matrix<double, 7, 1> Dynamics(const Eigen::Matrix<double, 7, 1>& x,
+                                       const Eigen::Matrix<double, 2, 1>& u);
+
+  class State {
+   public:
+    static constexpr int kX = 0;
+    static constexpr int kY = 1;
+    static constexpr int kHeading = 2;
+    static constexpr int kLeftVelocity = 3;
+    static constexpr int kRightVelocity = 4;
+    static constexpr int kLeftPosition = 5;
+    static constexpr int kRightPosition = 6;
+  };
+
+  /**
+   * Represents a gearing option of the Toughbox mini.
+   * 12.75:1 -- 14:50 and 14:50
+   * 10.71:1 -- 14:50 and 16:48
+   * 8.45:1 -- 14:50 and 19:45
+   * 7.31:1 -- 14:50 and 21:43
+   * 5.95:1 -- 14:50 and 24:40
+   */
+  class KitbotGearing {
+   public:
+    static constexpr double k12p75 = 12.75;
+    static constexpr double k10p71 = 10.71;
+    static constexpr double k8p45 = 8.45;
+    static constexpr double k7p31 = 7.31;
+    static constexpr double k5p95 = 5.95;
+  };
+
+  class KitbotMotor {
+   public:
+    static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1);
+    static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2);
+    static constexpr frc::DCMotor SingleMiniCIMPerSide =
+        frc::DCMotor::MiniCIM(1);
+    static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
+  };
+
+  class KitbotWheelSize {
+   public:
+    static constexpr units::meter_t kSixInch = 6_in;
+    static constexpr units::meter_t kEightInch = 8_in;
+    static constexpr units::meter_t kTenInch = 10_in;
+  };
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor     The motors installed in the bot.
+   * @param gearing   The gearing reduction used.
+   * @param wheelSize The wheel size.
+   */
+  static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor,
+                                                   double gearing,
+                                                   units::meter_t wheelSize) {
+    // MOI estimation -- note that I = m r^2 for point masses
+    units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in;
+    units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) *
+                                                2  // CIM plus toughbox per side
+                                                * (26_in / 2) * (26_in / 2);
+
+    return DifferentialDrivetrainSim{
+        motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in};
+  }
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor     The motors installed in the bot.
+   * @param gearing   The gearing reduction used.
+   * @param wheelSize The wheel size.
+   * @param J         The moment of inertia of the drivebase. This can be
+   * calculated using frc-characterization.
+   */
+  static DifferentialDrivetrainSim CreateKitbotSim(
+      frc::DCMotor motor, double gearing, units::meter_t wheelSize,
+      units::kilogram_square_meter_t J) {
+    return DifferentialDrivetrainSim{motor, gearing,         J,
+                                     25_kg, wheelSize / 2.0, 26_in};
+  }
+
+ private:
+  LinearSystem<2, 2, 2> m_plant;
+  units::meter_t m_rb;
+  units::meter_t m_wheelRadius;
+
+  DCMotor m_motor;
+
+  double m_originalGearing;
+  double m_currentGearing;
+
+  Eigen::Matrix<double, 7, 1> m_x;
+  Eigen::Matrix<double, 2, 1> m_u;
+};
+}  // namespace frc::sim

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h b/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h
new file mode 100644
index 0000000..2ff8a12
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DigitalOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated digital PWM output.
+ *
+ * This is for duty cycle PWM outputs on a DigitalOutput, not for the servo
+ * style PWM outputs on a PWM channel.
+ */
+class DigitalPWMSim {
+ public:
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param digitalOutput DigitalOutput to simulate
+   */
+  explicit DigitalPWMSim(const DigitalOutput& digitalOutput);
+
+  /**
+   * Creates an DigitalPWMSim for a digital I/O channel.
+   *
+   * @param channel DIO channel
+   * @return Simulated object
+   * @throws std::out_of_range if no Digital PWM is configured for that channel
+   */
+  static DigitalPWMSim CreateForChannel(int channel);
+
+  /**
+   * Creates an DigitalPWMSim for a simulated index.
+   * The index is incremented for each simulated DigitalPWM.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static DigitalPWMSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterDutyCycleCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetDutyCycle() const;
+
+  void SetDutyCycle(double dutyCycle);
+
+  std::unique_ptr<CallbackStore> RegisterPinCallback(NotifyCallback callback,
+                                                     bool initialNotify);
+
+  int GetPin() const;
+
+  void SetPin(int pin);
+
+  void ResetData();
+
+ private:
+  explicit DigitalPWMSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h b/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h
new file mode 100644
index 0000000..80cf942
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h
@@ -0,0 +1,248 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/DriverStationTypes.h>
+
+#include "frc/DriverStation.h"
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control a simulated driver station.
+ */
+class DriverStationSim {
+ public:
+  static std::unique_ptr<CallbackStore> RegisterEnabledCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetEnabled();
+
+  static void SetEnabled(bool enabled);
+
+  static std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetAutonomous();
+
+  static void SetAutonomous(bool autonomous);
+
+  static std::unique_ptr<CallbackStore> RegisterTestCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetTest();
+
+  static void SetTest(bool test);
+
+  static std::unique_ptr<CallbackStore> RegisterEStopCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetEStop();
+
+  static void SetEStop(bool eStop);
+
+  static std::unique_ptr<CallbackStore> RegisterFmsAttachedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetFmsAttached();
+
+  static void SetFmsAttached(bool fmsAttached);
+
+  static std::unique_ptr<CallbackStore> RegisterDsAttachedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetDsAttached();
+
+  static void SetDsAttached(bool dsAttached);
+
+  static std::unique_ptr<CallbackStore> RegisterAllianceStationIdCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static HAL_AllianceStationID GetAllianceStationId();
+
+  static void SetAllianceStationId(HAL_AllianceStationID allianceStationId);
+
+  static std::unique_ptr<CallbackStore> RegisterMatchTimeCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static double GetMatchTime();
+
+  static void SetMatchTime(double matchTime);
+
+  /**
+   * Updates DriverStation data so that new values are visible to the user
+   * program.
+   */
+  static void NotifyNewData();
+
+  /**
+   * Sets suppression of DriverStation::ReportError and ReportWarning messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendError(bool shouldSend);
+
+  /**
+   * Sets suppression of DriverStation::SendConsoleLine messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendConsoleLine(bool shouldSend);
+
+  /**
+   * Gets the joystick outputs.
+   *
+   * @param stick The joystick number
+   * @return The joystick outputs
+   */
+  static int64_t GetJoystickOutputs(int stick);
+
+  /**
+   * Gets the joystick rumble.
+   *
+   * @param stick The joystick number
+   * @param rumbleNum Rumble to get (0=left, 1=right)
+   * @return The joystick rumble value
+   */
+  static int GetJoystickRumble(int stick, int rumbleNum);
+
+  /**
+   * Sets the state of one joystick button. Button indexes begin at 1.
+   *
+   * @param stick The joystick number
+   * @param button The button index, beginning at 1
+   * @param state The state of the joystick button
+   */
+  static void SetJoystickButton(int stick, int button, bool state);
+
+  /**
+   * Gets the value of the axis on a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The analog axis number
+   * @param value The value of the axis on the joystick
+   */
+  static void SetJoystickAxis(int stick, int axis, double value);
+
+  /**
+   * Gets the state of a POV on a joystick.
+   *
+   * @param stick The joystick number
+   * @param pov The POV number
+   * @param value the angle of the POV in degrees, or -1 for not pressed
+   */
+  static void SetJoystickPOV(int stick, int pov, int value);
+
+  /**
+   * Sets the state of all the buttons on a joystick.
+   *
+   * @param stick The joystick number
+   * @param buttons The bitmap state of the buttons on the joystick
+   */
+  static void SetJoystickButtons(int stick, uint32_t buttons);
+
+  /**
+   * Sets the number of axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of axes on the indicated joystick
+   */
+  static void SetJoystickAxisCount(int stick, int count);
+
+  /**
+   * Sets the number of POVs for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of POVs on the indicated joystick
+   */
+  static void SetJoystickPOVCount(int stick, int count);
+
+  /**
+   * Sets the number of buttons for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of buttons on the indicated joystick
+   */
+  static void SetJoystickButtonCount(int stick, int count);
+
+  /**
+   * Sets the value of isXbox for a joystick.
+   *
+   * @param stick The joystick number
+   * @param isXbox The value of isXbox
+   */
+  static void SetJoystickIsXbox(int stick, bool isXbox);
+
+  /**
+   * Sets the value of type for a joystick.
+   *
+   * @param stick The joystick number
+   * @param type The value of type
+   */
+  static void SetJoystickType(int stick, int type);
+
+  /**
+   * Sets the name of a joystick.
+   *
+   * @param stick The joystick number
+   * @param name The value of name
+   */
+  static void SetJoystickName(int stick, const char* name);
+
+  /**
+   * Sets the types of Axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The target axis
+   * @param type The type of axis
+   */
+  static void SetJoystickAxisType(int stick, int axis, int type);
+
+  /**
+   * Sets the game specific message.
+   *
+   * @param message the game specific message
+   */
+  static void SetGameSpecificMessage(const char* message);
+
+  /**
+   * Sets the event name.
+   *
+   * @param name the event name
+   */
+  static void SetEventName(const char* name);
+
+  /**
+   * Sets the match type.
+   *
+   * @param type the match type
+   */
+  static void SetMatchType(DriverStation::MatchType type);
+
+  /**
+   * Sets the match number.
+   *
+   * @param matchNumber the match number
+   */
+  static void SetMatchNumber(int matchNumber);
+
+  /**
+   * Sets the replay number.
+   *
+   * @param replayNumber the replay number
+   */
+  static void SetReplayNumber(int replayNumber);
+
+  static void ResetData();
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
new file mode 100644
index 0000000..5d9a039
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/angle.h>
+
+namespace frc {
+
+class DutyCycleEncoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated duty cycle encoder.
+ */
+class DutyCycleEncoderSim {
+ public:
+  /**
+   * Constructs from a DutyCycleEncoder object.
+   *
+   * @param dutyCycleEncoder DutyCycleEncoder to simulate
+   */
+  explicit DutyCycleEncoderSim(const DutyCycleEncoder& encoder);
+
+  /**
+   * Set the position tin turns.
+   *
+   * @param turns The position.
+   */
+  void Set(units::turn_t turns);
+
+  /**
+   * Set the position.
+   */
+  void SetDistance(double distance);
+
+ private:
+  hal::SimDouble m_simPosition;
+  hal::SimDouble m_simDistancePerRotation;
+};
+
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h
new file mode 100644
index 0000000..40dad13
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DutyCycle;
+
+namespace sim {
+
+/**
+ * Class to control a simulated duty cycle digital input.
+ */
+class DutyCycleSim {
+ public:
+  /**
+   * Constructs from a DutyCycle object.
+   *
+   * @param dutyCycle DutyCycle to simulate
+   */
+  explicit DutyCycleSim(const DutyCycle& dutyCycle);
+
+  /**
+   * Creates a DutyCycleSim for a digital input channel.
+   *
+   * @param channel digital input channel
+   * @return Simulated object
+   * @throws std::out_of_range if no DutyCycle is configured for that channel
+   */
+  static DutyCycleSim CreateForChannel(int channel);
+
+  /**
+   * Creates a DutyCycleSim for a simulated index.
+   * The index is incremented for each simulated DutyCycle.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static DutyCycleSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetFrequency() const;
+
+  void SetFrequency(int count);
+
+  std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  double GetOutput() const;
+
+  void SetOutput(double period);
+
+  void ResetData();
+
+ private:
+  explicit DutyCycleSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
new file mode 100644
index 0000000..9b7069f
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/velocity.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated elevator mechanism.
+ */
+class ElevatorSim : public LinearSystemSim<2, 1, 1> {
+ public:
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param plant              The linear system that represents the elevator.
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param gearing            The gearing of the elevator (numbers greater
+   *                           than 1 represent reductions).
+   * @param drumRadius         The radius of the drum that your cable is
+   *                           wrapped around.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
+              double gearing, units::meter_t drumRadius,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param gearing            The gearing of the elevator (numbers greater
+   *                           than 1 represent reductions).
+   * @param carriageMass       The mass of the elevator carriage.
+   * @param drumRadius         The radius of the drum that your cable is
+   *                           wrapped around.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  ElevatorSim(const DCMotor& gearbox, double gearing,
+              units::kilogram_t carriageMass, units::meter_t drumRadius,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Returns whether the elevator has hit the lower limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the lower limit.
+   */
+  bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns whether the elevator has hit the upper limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the upper limit.
+   */
+  bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns the position of the elevator.
+   *
+   * @return The position of the elevator.
+   */
+  units::meter_t GetPosition() const;
+
+  /**
+   * Returns the velocity of the elevator.
+   *
+   * @return The velocity of the elevator.
+   */
+  units::meters_per_second_t GetVelocity() const;
+
+  /**
+   * Returns the elevator current draw.
+   *
+   * @return The elevator current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+ protected:
+  /**
+   * Updates the state estimate of the elevator.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  Eigen::Matrix<double, 2, 1> UpdateX(
+      const Eigen::Matrix<double, 2, 1>& currentXhat,
+      const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
+
+ private:
+  DCMotor m_gearbox;
+  units::meter_t m_drumRadius;
+  units::meter_t m_minHeight;
+  units::meter_t m_maxHeight;
+  double m_gearing;
+};
+}  // namespace frc::sim

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h
new file mode 100644
index 0000000..4049ab8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Encoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated encoder.
+ */
+class EncoderSim {
+ public:
+  /**
+   * Constructs from an Encoder object.
+   *
+   * @param encoder Encoder to simulate
+   */
+  explicit EncoderSim(const Encoder& encoder);
+
+  /**
+   * Creates an EncoderSim for a digital input channel.  Encoders take two
+   * channels, so either one may be specified.
+   *
+   * @param channel digital input channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no Encoder is configured for that channel
+   */
+  static EncoderSim CreateForChannel(int channel);
+
+  /**
+   * Creates an EncoderSim for a simulated index.
+   * The index is incremented for each simulated Encoder.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static EncoderSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterCountCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  int GetCount() const;
+
+  void SetCount(int count);
+
+  std::unique_ptr<CallbackStore> RegisterPeriodCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  double GetPeriod() const;
+
+  void SetPeriod(double period);
+
+  std::unique_ptr<CallbackStore> RegisterResetCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  bool GetReset() const;
+
+  void SetReset(bool reset);
+
+  std::unique_ptr<CallbackStore> RegisterMaxPeriodCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetMaxPeriod() const;
+
+  void SetMaxPeriod(double maxPeriod);
+
+  std::unique_ptr<CallbackStore> RegisterDirectionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetDirection() const;
+
+  void SetDirection(bool direction);
+
+  std::unique_ptr<CallbackStore> RegisterReverseDirectionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetReverseDirection() const;
+
+  void SetReverseDirection(bool reverseDirection);
+
+  std::unique_ptr<CallbackStore> RegisterSamplesToAverageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetSamplesToAverage() const;
+
+  void SetSamplesToAverage(int samplesToAverage);
+
+  std::unique_ptr<CallbackStore> RegisterDistancePerPulseCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetDistancePerPulse() const;
+
+  void SetDistancePerPulse(double distancePerPulse);
+
+  void ResetData();
+
+  void SetDistance(double distance);
+
+  double GetDistance();
+
+  void SetRate(double rate);
+
+  double GetRate();
+
+ private:
+  explicit EncoderSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/Field2d.h b/wpilibc/src/main/native/include/frc/simulation/Field2d.h
new file mode 100644
index 0000000..de30847
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/Field2d.h
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+/**
+ * 2D representation of game field (for simulation).
+ *
+ * In non-simulation mode this simply stores and returns the robot pose.
+ *
+ * The robot pose is the actual location shown on the simulation view.
+ * This may or may not match the robot's internal odometry.  For example, if
+ * the robot is shown at a particular starting location, the pose in this
+ * class would represent the actual location on the field, but the robot's
+ * internal state might have a 0,0,0 pose (unless it's initialized to
+ * something different).
+ *
+ * As the user is able to edit the pose, code performing updates should get
+ * the robot pose, transform it as appropriate (e.g. based on simulated wheel
+ * velocity), and set the new pose.
+ */
+class Field2d {
+ public:
+  Field2d();
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetRobotPose(const Pose2d& pose);
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  Pose2d GetRobotPose();
+
+ private:
+  Pose2d m_pose;
+
+  hal::SimDevice m_device;
+  hal::SimDouble m_x;
+  hal::SimDouble m_y;
+  hal::SimDouble m_rot;
+};
+
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
new file mode 100644
index 0000000..1c47c45
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/angular_velocity.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated flywheel mechanism.
+ */
+class FlywheelSim : public LinearSystemSim<1, 1, 1> {
+ public:
+  /**
+   * Creates a simulated flywhel mechanism.
+   *
+   * @param plant              The linear system representing the flywheel.
+   * @param gearbox            The type of and number of motors in the flywheel
+   *                           gearbox.
+   * @param gearing            The gearing of the flywheel (numbers greater than
+   *                           1 represent reductions).
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox,
+              double gearing,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Creates a simulated flywhel mechanism.
+   *
+   * @param gearbox            The type of and number of motors in the flywheel
+   *                           gearbox.
+   * @param gearing            The gearing of the flywheel (numbers greater than
+   *                           1 represent reductions).
+   * @param moi                The moment of inertia of the flywheel.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  FlywheelSim(const DCMotor& gearbox, double gearing,
+              units::kilogram_square_meter_t moi,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Returns the flywheel velocity.
+   *
+   * @return The flywheel velocity.
+   */
+  units::radians_per_second_t GetAngularVelocity() const;
+
+  /**
+   * Returns the flywheel current draw.
+   *
+   * @return The flywheel current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the flywheel.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+ private:
+  DCMotor m_gearbox;
+  double m_gearing;
+};
+}  // namespace frc::sim

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h b/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
new file mode 100644
index 0000000..1705200
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include "frc/GenericHID.h"
+
+namespace frc {
+
+class GenericHID;
+
+namespace sim {
+
+/**
+ * Class to control a simulated generic joystick.
+ */
+class GenericHIDSim {
+ public:
+  /**
+   * Constructs from a GenericHID object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit GenericHIDSim(const GenericHID& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit GenericHIDSim(int port);
+
+  /**
+   * Updates joystick data so that new values are visible to the user program.
+   */
+  void NotifyNewData();
+
+  void SetRawButton(int button, bool value);
+
+  void SetRawAxis(int axis, double value);
+
+  void SetPOV(int pov, int value);
+
+  void SetPOV(int value);
+
+  void SetAxisCount(int count);
+
+  void SetPOVCount(int count);
+
+  void SetButtonCount(int count);
+
+  void SetType(GenericHID::HIDType type);
+
+  void SetName(const char* name);
+
+  void SetAxisType(int axis, int type);
+
+  bool GetOutput(int outputNumber);
+
+  int64_t GetOutputs();
+
+  double GetRumble(GenericHID::RumbleType type);
+
+ protected:
+  int m_port;
+};
+
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h b/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h
new file mode 100644
index 0000000..5735728
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/simulation/GenericHIDSim.h"
+
+namespace frc {
+
+class Joystick;
+
+namespace sim {
+
+/**
+ * Class to control a simulated joystick.
+ */
+class JoystickSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a Joystick object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit JoystickSim(const Joystick& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit JoystickSim(int port);
+
+  void SetX(double value);
+
+  void SetY(double value);
+
+  void SetZ(double value);
+
+  void SetTwist(double value);
+
+  void SetThrottle(double value);
+
+  void SetTrigger(bool state);
+
+  void SetTop(bool state);
+
+ private:
+  const Joystick* m_joystick = nullptr;
+};
+
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
new file mode 100644
index 0000000..4f3c658
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <Eigen/Core>
+#include <units/current.h>
+#include <units/time.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/LinearSystem.h"
+
+namespace frc::sim {
+/**
+ * This class helps simulate linear systems. To use this class, do the following
+ * in the simulationPeriodic() method.
+ *
+ * Call the SetInput() method with the inputs to your system (generally
+ * voltage). Call the Update() method to update the simulation. Set simulated
+ * sensor readings with the simulated positions in the GetOutput() method.
+ *
+ * @tparam States  The number of states of the system.
+ * @tparam Inputs  The number of inputs to the system.
+ * @tparam Outputs The number of outputs of the system.
+ */
+template <int States, int Inputs, int Outputs>
+class LinearSystemSim {
+ public:
+  /**
+   * Creates a simulated generic linear system.
+   *
+   * @param system             The system to simulate.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  LinearSystemSim(const LinearSystem<States, Inputs, Outputs>& system,
+                  const std::array<double, Outputs>& measurementStdDevs =
+                      std::array<double, Outputs>{})
+      : m_plant(system), m_measurementStdDevs(measurementStdDevs) {
+    m_x = Eigen::Matrix<double, States, 1>::Zero();
+    m_y = Eigen::Matrix<double, Outputs, 1>::Zero();
+    m_u = Eigen::Matrix<double, Inputs, 1>::Zero();
+  }
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dt The time between updates.
+   */
+  void Update(units::second_t dt) {
+    // Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
+    // Bu_k
+    m_x = UpdateX(m_x, m_u, dt);
+
+    // y = Cx + Du
+    m_y = m_plant.CalculateY(m_x, m_u);
+
+    // Add noise. If the user did not pass a noise vector to the
+    // constructor, then this method will not do anything because
+    // the standard deviations default to zero.
+    m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
+  }
+
+  /**
+   * Returns the current output of the plant.
+   *
+   * @return The current output of the plant.
+   */
+  const Eigen::Matrix<double, Outputs, 1>& GetOutput() const { return m_y; }
+
+  /**
+   * Returns an element of the current output of the plant.
+   *
+   * @param row The row to return.
+   * @return An element of the current output of the plant.
+   */
+  double GetOutput(int row) const { return m_y(row); }
+
+  /**
+   * Sets the system inputs (usually voltages).
+   *
+   * @param u The system inputs.
+   */
+  void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
+
+  /*
+   * Sets the system inputs.
+   *
+   * @param row   The row in the input matrix to set.
+   * @param value The value to set the row to.
+   */
+  void SetInput(int row, double value) { m_u(row, 0) = value; }
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The new state.
+   */
+  void SetState(const Eigen::Matrix<double, States, 1>& state) { m_x = state; }
+
+  /**
+   * Returns the current drawn by this simulated system. Override this method to
+   * add a custom current calculation.
+   *
+   * @return The current drawn by this simulated mechanism.
+   */
+  virtual units::ampere_t GetCurrentDraw() const {
+    return units::ampere_t(0.0);
+  }
+
+ protected:
+  /**
+   * Updates the state estimate of the system.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (usually voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  virtual Eigen::Matrix<double, States, 1> UpdateX(
+      const Eigen::Matrix<double, States, 1>& currentXhat,
+      const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+    return m_plant.CalculateX(currentXhat, u, dt);
+  }
+
+  LinearSystem<States, Inputs, Outputs> m_plant;
+
+  Eigen::Matrix<double, States, 1> m_x;
+  Eigen::Matrix<double, Outputs, 1> m_y;
+  Eigen::Matrix<double, Inputs, 1> m_u;
+  std::array<double, Outputs> m_measurementStdDevs;
+};
+}  // namespace frc::sim

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h b/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h
new file mode 100644
index 0000000..3d61cf5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <hal/SimDevice.h>
+#include <wpi/StringMap.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+class Mechanism2D {
+ public:
+  Mechanism2D();
+
+  /**
+   * Set/Create the angle of a ligament
+   *
+   * @param ligamentPath json path to the ligament
+   * @param angle to set the ligament
+   */
+  void SetLigamentAngle(const wpi::Twine& ligamentPath, float angle);
+
+  /**
+   * Set/Create the length of a ligament
+   *
+   * @param ligamentPath json path to the ligament
+   * @param length to set the ligament
+   */
+  void SetLigamentLength(const wpi::Twine& ligamentPath, float length);
+
+ private:
+  wpi::StringMap<hal::SimDouble> createdItems;
+  hal::SimDevice m_device;
+};
+
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/PCMSim.h b/wpilibc/src/main/native/include/frc/simulation/PCMSim.h
new file mode 100644
index 0000000..2e3ed5e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/PCMSim.h
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Compressor;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class PCMSim {
+ public:
+  /**
+   * Constructs with the default PCM module number (CAN ID).
+   */
+  PCMSim();
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit PCMSim(int module);
+
+  /**
+   * Constructs from a Compressor object.
+   *
+   * @param compressor Compressor connected to PCM to simulate
+   */
+  explicit PCMSim(const Compressor& compressor);
+
+  std::unique_ptr<CallbackStore> RegisterSolenoidInitializedCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  bool GetSolenoidInitialized(int channel) const;
+
+  void SetSolenoidInitialized(int channel, bool solenoidInitialized);
+
+  std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  bool GetSolenoidOutput(int channel) const;
+
+  void SetSolenoidOutput(int channel, bool solenoidOutput);
+
+  std::unique_ptr<CallbackStore> RegisterCompressorInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetCompressorInitialized() const;
+
+  void SetCompressorInitialized(bool compressorInitialized);
+
+  std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetCompressorOn() const;
+
+  void SetCompressorOn(bool compressorOn);
+
+  std::unique_ptr<CallbackStore> RegisterClosedLoopEnabledCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetClosedLoopEnabled() const;
+
+  void SetClosedLoopEnabled(bool closedLoopEnabled);
+
+  std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetPressureSwitch() const;
+
+  void SetPressureSwitch(bool pressureSwitch);
+
+  std::unique_ptr<CallbackStore> RegisterCompressorCurrentCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetCompressorCurrent() const;
+
+  void SetCompressorCurrent(double compressorCurrent);
+
+  uint8_t GetAllSolenoidOutputs() const;
+
+  void SetAllSolenoidOutputs(uint8_t outputs);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/PDPSim.h b/wpilibc/src/main/native/include/frc/simulation/PDPSim.h
new file mode 100644
index 0000000..c67815f
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/PDPSim.h
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class PowerDistributionPanel;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Power Distribution Panel (PDP).
+ */
+class PDPSim {
+ public:
+  /**
+   * Constructs from a PDP module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit PDPSim(int module = 0);
+
+  /**
+   * Constructs from a PowerDistributionPanel object.
+   *
+   * @param pdp PowerDistributionPanel to simulate
+   */
+  explicit PDPSim(const PowerDistributionPanel& pdp);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterTemperatureCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetTemperature() const;
+
+  void SetTemperature(double temperature);
+
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetVoltage() const;
+
+  void SetVoltage(double voltage);
+
+  std::unique_ptr<CallbackStore> RegisterCurrentCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  double GetCurrent(int channel) const;
+
+  void SetCurrent(int channel, double current);
+
+  void GetAllCurrents(double* currents) const;
+
+  void SetAllCurrents(const double* currents);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/PWMSim.h b/wpilibc/src/main/native/include/frc/simulation/PWMSim.h
new file mode 100644
index 0000000..ca2dfca
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/PWMSim.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class PWM;
+
+namespace sim {
+
+/**
+ * Class to control a simulated PWM output.
+ */
+class PWMSim {
+ public:
+  /**
+   * Constructs from a PWM object.
+   *
+   * @param pwm PWM to simulate
+   */
+  explicit PWMSim(const PWM& pwm);
+
+  /**
+   * Constructs from a PWM channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit PWMSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterRawValueCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetRawValue() const;
+
+  void SetRawValue(int rawValue);
+
+  std::unique_ptr<CallbackStore> RegisterSpeedCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  double GetSpeed() const;
+
+  void SetSpeed(double speed);
+
+  std::unique_ptr<CallbackStore> RegisterPositionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetPosition() const;
+
+  void SetPosition(double position);
+
+  std::unique_ptr<CallbackStore> RegisterPeriodScaleCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetPeriodScale() const;
+
+  void SetPeriodScale(int periodScale);
+
+  std::unique_ptr<CallbackStore> RegisterZeroLatchCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetZeroLatch() const;
+
+  void SetZeroLatch(bool zeroLatch);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/RelaySim.h b/wpilibc/src/main/native/include/frc/simulation/RelaySim.h
new file mode 100644
index 0000000..99a8eef
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/RelaySim.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Relay;
+
+namespace sim {
+
+/**
+ * Class to control a simulated relay.
+ */
+class RelaySim {
+ public:
+  /**
+   * Constructs from a Relay object.
+   *
+   * @param relay Relay to simulate
+   */
+  explicit RelaySim(const Relay& relay);
+
+  /**
+   * Constructs from a relay channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit RelaySim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedForwardCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitializedForward() const;
+
+  void SetInitializedForward(bool initializedForward);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedReverseCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitializedReverse() const;
+
+  void SetInitializedReverse(bool initializedReverse);
+
+  std::unique_ptr<CallbackStore> RegisterForwardCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetForward() const;
+
+  void SetForward(bool forward);
+
+  std::unique_ptr<CallbackStore> RegisterReverseCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetReverse() const;
+
+  void SetReverse(bool reverse);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h b/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
new file mode 100644
index 0000000..32eb462
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <units/current.h>
+#include <units/voltage.h>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control a simulated RoboRIO.
+ */
+class RoboRioSim {
+ public:
+  static std::unique_ptr<CallbackStore> RegisterFPGAButtonCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetFPGAButton();
+
+  static void SetFPGAButton(bool fPGAButton);
+
+  static std::unique_ptr<CallbackStore> RegisterVInVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetVInVoltage();
+
+  static void SetVInVoltage(units::volt_t vInVoltage);
+
+  static std::unique_ptr<CallbackStore> RegisterVInCurrentCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetVInCurrent();
+
+  static void SetVInCurrent(units::ampere_t vInCurrent);
+
+  static std::unique_ptr<CallbackStore> RegisterUserVoltage6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetUserVoltage6V();
+
+  static void SetUserVoltage6V(units::volt_t userVoltage6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserCurrent6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetUserCurrent6V();
+
+  static void SetUserCurrent6V(units::ampere_t userCurrent6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserActive6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetUserActive6V();
+
+  static void SetUserActive6V(bool userActive6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserVoltage5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetUserVoltage5V();
+
+  static void SetUserVoltage5V(units::volt_t userVoltage5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserCurrent5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetUserCurrent5V();
+
+  static void SetUserCurrent5V(units::ampere_t userCurrent5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserActive5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetUserActive5V();
+
+  static void SetUserActive5V(bool userActive5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserVoltage3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetUserVoltage3V3();
+
+  static void SetUserVoltage3V3(units::volt_t userVoltage3V3);
+
+  static std::unique_ptr<CallbackStore> RegisterUserCurrent3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetUserCurrent3V3();
+
+  static void SetUserCurrent3V3(units::ampere_t userCurrent3V3);
+
+  static std::unique_ptr<CallbackStore> RegisterUserActive3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetUserActive3V3();
+
+  static void SetUserActive3V3(bool userActive3V3);
+
+  static std::unique_ptr<CallbackStore> RegisterUserFaults6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static int GetUserFaults6V();
+
+  static void SetUserFaults6V(int userFaults6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserFaults5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static int GetUserFaults5V();
+
+  static void SetUserFaults5V(int userFaults5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserFaults3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static int GetUserFaults3V3();
+
+  static void SetUserFaults3V3(int userFaults3V3);
+
+  static void ResetData();
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h b/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
new file mode 100644
index 0000000..f2e3249
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+namespace sim {
+class SPIAccelerometerSim {
+ public:
+  explicit SPIAccelerometerSim(int index);
+
+  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  bool GetActive() const;
+
+  void SetActive(bool active);
+
+  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  int GetRange() const;
+
+  void SetRange(int range);
+
+  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetX() const;
+
+  void SetX(double x);
+
+  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetY() const;
+
+  void SetY(double y);
+
+  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetZ() const;
+
+  void SetZ(double z);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h b/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h
new file mode 100644
index 0000000..640cc0e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <string>
+#include <vector>
+
+#include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control the simulation side of a SimDevice.
+ */
+class SimDeviceSim {
+ public:
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   */
+  explicit SimDeviceSim(const char* name);
+
+  hal::SimValue GetValue(const char* name) const;
+
+  hal::SimDouble GetDouble(const char* name) const;
+
+  hal::SimEnum GetEnum(const char* name) const;
+
+  hal::SimBoolean GetBoolean(const char* name) const;
+
+  static std::vector<std::string> GetEnumOptions(hal::SimEnum val);
+
+  template <typename F>
+  void EnumerateValues(F callback) const {
+    return HALSIM_EnumerateSimValues(
+        m_handle, &callback,
+        [](const char* name, void* param, HAL_SimValueHandle handle,
+           HAL_Bool readonly, const struct HAL_Value* value) {
+          std::invoke(*static_cast<F*>(param), name, handle, readonly, value);
+        });
+  }
+
+  operator HAL_SimDeviceHandle() const { return m_handle; }
+
+  template <typename F>
+  static void EnumerateDevices(const char* prefix, F callback) {
+    return HALSIM_EnumerateSimDevices(
+        prefix, &callback,
+        [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+          std::invoke(*static_cast<F*>(param), name, handle);
+        });
+  }
+
+  static void ResetData();
+
+ private:
+  HAL_SimDeviceHandle m_handle;
+};
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/SimHooks.h b/wpilibc/src/main/native/include/frc/simulation/SimHooks.h
new file mode 100644
index 0000000..7690a4f
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SimHooks.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <hal/HALBase.h>
+#include <units/time.h>
+
+namespace frc {
+namespace sim {
+
+void SetRuntimeType(HAL_RuntimeType type);
+
+void WaitForProgramStart();
+
+void SetProgramStarted();
+
+bool GetProgramStarted();
+
+void RestartTiming();
+
+void PauseTiming();
+
+void ResumeTiming();
+
+bool IsTimingPaused();
+
+void StepTiming(units::second_t delta);
+
+void StepTimingAsync(units::second_t delta);
+
+}  // namespace sim
+}  // namespace frc

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
new file mode 100644
index 0000000..f8de822
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated arm mechanism.
+ */
+class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
+ public:
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param system             The system representing this arm.
+   * @param gearbox            The type and number of motors on the arm gearbox.
+   * @param gearing            The gear ratio of the arm (numbers greater than 1
+   *                           represent reductions).
+   * @param armLength          The length of the arm.
+   * @param minAngle           The minimum angle that the arm is capable of.
+   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param mass               The mass of the arm.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   */
+  SingleJointedArmSim(const LinearSystem<2, 1, 1>& system,
+                      const DCMotor& gearbox, double gearing,
+                      units::meter_t armLength, units::radian_t minAngle,
+                      units::radian_t maxAngle, units::kilogram_t mass,
+                      bool simulateGravity,
+                      const std::array<double, 1>& measurementStdDevs = {0.0});
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox            The type and number of motors on the arm gearbox.
+   * @param gearing            The gear ratio of the arm (numbers greater than 1
+   *                           represent reductions).
+   * @param moi                The moment of inertia of the arm. This can be
+   *                           calculated from CAD software.
+   * @param armLength          The length of the arm.
+   * @param minAngle           The minimum angle that the arm is capable of.
+   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param mass               The mass of the arm.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   */
+  SingleJointedArmSim(const DCMotor& gearbox, double gearing,
+                      units::kilogram_square_meter_t moi,
+                      units::meter_t armLength, units::radian_t minAngle,
+                      units::radian_t maxAngle, units::kilogram_t mass,
+                      bool simulateGravity,
+                      const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Returns whether the arm has hit the lower limit.
+   *
+   * @param x The current arm state.
+   * @return Whether the arm has hit the lower limit.
+   */
+  bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns whether the arm has hit the upper limit.
+   *
+   * @param x The current arm state.
+   * @return Whether the arm has hit the upper limit.
+   */
+  bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns the current arm angle.
+   *
+   * @return The current arm angle.
+   */
+  units::radian_t GetAngle() const;
+
+  /**
+   * Returns the current arm velocity.
+   *
+   * @return The current arm velocity.
+   */
+  units::radians_per_second_t GetVelocity() const;
+
+  /**
+   * Returns the arm current draw.
+   *
+   * @return The arm current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+  /**
+   * Calculates a rough estimate of the moment of inertia of an arm given its
+   * length and mass.
+   *
+   * @param length The length of the arm.
+   * @param mass   The mass of the arm.
+   *
+   * @return The calculated moment of inertia.
+   */
+  static constexpr units::kilogram_square_meter_t EstimateMOI(
+      units::meter_t length, units::kilogram_t mass) {
+    return 1.0 / 3.0 * mass * length * length;
+  }
+
+ protected:
+  /**
+   * Updates the state estimate of the arm.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  Eigen::Matrix<double, 2, 1> UpdateX(
+      const Eigen::Matrix<double, 2, 1>& currentXhat,
+      const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
+
+ private:
+  units::meter_t m_r;
+  units::radian_t m_minAngle;
+  units::radian_t m_maxAngle;
+  units::kilogram_t m_mass;
+  const DCMotor m_gearbox;
+  double m_gearing;
+  bool m_simulateGravity;
+};
+}  // namespace frc::sim

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

diff --git a/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h b/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h
new file mode 100644
index 0000000..d981111
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/simulation/GenericHIDSim.h"
+
+namespace frc {
+
+class XboxController;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Xbox 360 or Xbox One controller.
+ */
+class XboxControllerSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a XboxController object.
+   *
+   * @param joystick controller to simulate
+   */
+  explicit XboxControllerSim(const XboxController& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit XboxControllerSim(int port);
+
+  void SetX(GenericHID::JoystickHand hand, double value);
+
+  void SetY(GenericHID::JoystickHand hand, double value);
+
+  void SetTriggerAxis(GenericHID::JoystickHand hand, double value);
+
+  void SetBumper(GenericHID::JoystickHand hand, bool state);
+
+  void SetStickButton(GenericHID::JoystickHand hand, bool state);
+
+  void SetAButton(bool state);
+
+  void SetBButton(bool state);
+
+  void SetXButton(bool state);
+
+  void SetYButton(bool state);
+
+  void SetBackButton(bool state);
+
+  void SetStartButton(bool state);
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h

diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
index a0ea0f9..98419b9 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,7 +17,9 @@
 class SendableBase : public Sendable, public SendableHelper<SendableBase> {
  public:
   /**
-   * Creates an instance of the sensor base.
+   * Creates an instance of the sensor base
+   *
+   * @deprecated use Sendable and SendableHelper
    *
    * @param addLiveWindow if true, add this Sendable to LiveWindow
    */

wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h

diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
index eb69dcd..e5dea44 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -202,7 +202,7 @@
 
   std::vector<Property> m_properties;
   std::function<void()> m_safeState;
-  std::function<void()> m_updateTable;
+  std::vector<std::function<void()>> m_updateTables;
   std::shared_ptr<nt::NetworkTable> m_table;
   nt::NetworkTableEntry m_controllableEntry;
   bool m_actuator = false;

wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h

diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
index 6cf3c47..b370fc0 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -46,13 +46,38 @@
 
  public:
   ~SendableChooser() override = default;
+  SendableChooser() = default;
+  SendableChooser(SendableChooser&& rhs) = default;
+  SendableChooser& operator=(SendableChooser&& rhs) = default;
 
   void AddOption(wpi::StringRef name, T object);
   void SetDefaultOption(wpi::StringRef name, T object);
 
+  /**
+   * Adds the given object to the list of options.
+   *
+   * On the SmartDashboard on the desktop, the object will appear as the given
+   * name.
+   *
+   * @deprecated use AddOption(wpi::StringRef name, T object) instead.
+   *
+   * @param name   the name of the option
+   * @param object the option
+   */
   WPI_DEPRECATED("use AddOption() instead")
   void AddObject(wpi::StringRef name, T object) { AddOption(name, object); }
 
+  /**
+   * Add the given object to the list of options and marks it as the default.
+   *
+   * Functionally, this is very close to AddOption() except that it will use
+   * this as the default option if none other is explicitly selected.
+   *
+   * @deprecated use SetDefaultOption(wpi::StringRef name, T object) instead.
+   *
+   * @param name   the name of the option
+   * @param object the option
+   */
   WPI_DEPRECATED("use SetDefaultOption() instead")
   void AddDefault(wpi::StringRef name, T object) {
     SetDefaultOption(name, object);

wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc

diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
index 295d263..57e2828 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -76,20 +76,21 @@
 void SendableChooser<T>::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("String Chooser");
   builder.GetEntry(kInstance).SetDouble(m_instance);
-  builder.AddStringArrayProperty(kOptions,
-                                 [=]() {
-                                   std::vector<std::string> keys;
-                                   for (const auto& choice : m_choices) {
-                                     keys.push_back(choice.first());
-                                   }
+  builder.AddStringArrayProperty(
+      kOptions,
+      [=]() {
+        std::vector<std::string> keys;
+        for (const auto& choice : m_choices) {
+          keys.push_back(choice.first());
+        }
 
-                                   // Unlike std::map, wpi::StringMap elements
-                                   // are not sorted
-                                   std::sort(keys.begin(), keys.end());
+        // Unlike std::map, wpi::StringMap elements
+        // are not sorted
+        std::sort(keys.begin(), keys.end());
 
-                                   return keys;
-                                 },
-                                 nullptr);
+        return keys;
+      },
+      nullptr);
   builder.AddSmallStringProperty(
       kDefault,
       [=](wpi::SmallVectorImpl<char>&) -> wpi::StringRef {

wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h

diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
index 6b0b28b..1f39216 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -46,6 +46,8 @@
   /**
    * Gets the name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::GetName()
+   *
    * @return Name
    */
   WPI_DEPRECATED("use SendableRegistry::GetName()")
@@ -57,6 +59,8 @@
   /**
    * Sets the name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param name name
    */
   WPI_DEPRECATED("use SendableRegistry::SetName()")
@@ -67,6 +71,8 @@
   /**
    * Sets both the subsystem name and device name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param subsystem subsystem name
    * @param name device name
    */
@@ -79,6 +85,8 @@
   /**
    * Gets the subsystem name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::GetSubsystem().
+   *
    * @return Subsystem name
    */
   WPI_DEPRECATED("use SendableRegistry::GetSubsystem()")
@@ -90,6 +98,8 @@
   /**
    * Sets the subsystem name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::SetSubsystem()
+   *
    * @param subsystem subsystem name
    */
   WPI_DEPRECATED("use SendableRegistry::SetSubsystem()")
@@ -102,6 +112,8 @@
   /**
    * Add a child component.
    *
+   * @deprecated use SendableRegistry::AddChild()
+   *
    * @param child child component
    */
   WPI_DEPRECATED("use SendableRegistry::AddChild()")
@@ -113,6 +125,8 @@
   /**
    * Add a child component.
    *
+   * @deprecated use SendableRegistry::AddChild()
+   *
    * @param child child component
    */
   WPI_DEPRECATED("use SendableRegistry::AddChild()")
@@ -124,6 +138,8 @@
   /**
    * Sets the name of the sensor with a channel number.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param moduleType A string that defines the module name in the label for
    *                   the value
    * @param channel    The channel number the device is plugged into
@@ -137,6 +153,8 @@
   /**
    * Sets the name of the sensor with a module and channel number.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param moduleType   A string that defines the module name in the label for
    *                     the value
    * @param moduleNumber The number of the particular module type

wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h

diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
deleted file mode 100644
index 1e08a94..0000000
--- a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <array>
-
-#include <Eigen/Core>
-
-#include "frc/spline/Spline.h"
-
-namespace frc {
-/**
- * Represents a hermite spline of degree 3.
- */
-class CubicHermiteSpline : public Spline<3> {
- public:
-  /**
-   * Constructs a cubic hermite spline with the specified control vectors. Each
-   * control vector contains info about the location of the point and its first
-   * derivative.
-   *
-   * @param xInitialControlVector The control vector for the initial point in
-   * the x dimension.
-   * @param xFinalControlVector The control vector for the final point in
-   * the x dimension.
-   * @param yInitialControlVector The control vector for the initial point in
-   * the y dimension.
-   * @param yFinalControlVector The control vector for the final point in
-   * the y dimension.
-   */
-  CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
-                     std::array<double, 2> xFinalControlVector,
-                     std::array<double, 2> yInitialControlVector,
-                     std::array<double, 2> yFinalControlVector);
-
- protected:
-  /**
-   * Returns the coefficients matrix.
-   * @return The coefficients matrix.
-   */
-  Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
-    return m_coefficients;
-  }
-
- private:
-  Eigen::Matrix<double, 6, 4> m_coefficients =
-      Eigen::Matrix<double, 6, 4>::Zero();
-
-  /**
-   * Returns the hermite basis matrix for cubic hermite spline interpolation.
-   * @return The hermite basis matrix for cubic hermite spline interpolation.
-   */
-  static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
-    // clang-format off
-    static auto basis = (Eigen::Matrix<double, 4, 4>() <<
-     +2.0, +1.0, -2.0, +1.0,
-     -3.0, -2.0, +3.0, -1.0,
-     +0.0, +1.0, +0.0, +0.0,
-     +1.0, +0.0, +0.0, +0.0).finished();
-    // clang-format on
-    return basis;
-  }
-
-  /**
-   * Returns the control vector for each dimension as a matrix from the
-   * user-provided arrays in the constructor.
-   *
-   * @param initialVector The control vector for the initial point.
-   * @param finalVector The control vector for the final point.
-   *
-   * @return The control vector matrix for a dimension.
-   */
-  static Eigen::Vector4d ControlVectorFromArrays(
-      std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
-    return (Eigen::Vector4d() << initialVector[0], initialVector[1],
-            finalVector[0], finalVector[1])
-        .finished();
-  }
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h

diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
deleted file mode 100644
index 632d3ad..0000000
--- a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <array>
-
-#include <Eigen/Core>
-
-#include "frc/spline/Spline.h"
-
-namespace frc {
-/**
- * Represents a hermite spline of degree 5.
- */
-class QuinticHermiteSpline : public Spline<5> {
- public:
-  /**
-   * Constructs a quintic hermite spline with the specified control vectors.
-   * Each control vector contains into about the location of the point, its
-   * first derivative, and its second derivative.
-   *
-   * @param xInitialControlVector The control vector for the initial point in
-   * the x dimension.
-   * @param xFinalControlVector The control vector for the final point in
-   * the x dimension.
-   * @param yInitialControlVector The control vector for the initial point in
-   * the y dimension.
-   * @param yFinalControlVector The control vector for the final point in
-   * the y dimension.
-   */
-  QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
-                       std::array<double, 3> xFinalControlVector,
-                       std::array<double, 3> yInitialControlVector,
-                       std::array<double, 3> yFinalControlVector);
-
- protected:
-  /**
-   * Returns the coefficients matrix.
-   * @return The coefficients matrix.
-   */
-  Eigen::Matrix<double, 6, 6> Coefficients() const override {
-    return m_coefficients;
-  }
-
- private:
-  Eigen::Matrix<double, 6, 6> m_coefficients =
-      Eigen::Matrix<double, 6, 6>::Zero();
-
-  /**
-   * Returns the hermite basis matrix for quintic hermite spline interpolation.
-   * @return The hermite basis matrix for quintic hermite spline interpolation.
-   */
-  static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
-    // clang-format off
-    static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
-      -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
-      +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
-      -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
-      +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
-      +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
-      +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
-    // clang-format on
-    return basis;
-  }
-
-  /**
-   * Returns the control vector for each dimension as a matrix from the
-   * user-provided arrays in the constructor.
-   *
-   * @param initialVector The control vector for the initial point.
-   * @param finalVector The control vector for the final point.
-   *
-   * @return The control vector matrix for a dimension.
-   */
-  static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
-      std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
-    return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
-            initialVector[2], finalVector[0], finalVector[1], finalVector[2])
-        .finished();
-  }
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/spline/Spline.h

diff --git a/wpilibc/src/main/native/include/frc/spline/Spline.h b/wpilibc/src/main/native/include/frc/spline/Spline.h
deleted file mode 100644
index e8f2372..0000000
--- a/wpilibc/src/main/native/include/frc/spline/Spline.h
+++ /dev/null
@@ -1,135 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <array>
-#include <utility>
-#include <vector>
-
-#include <Eigen/Core>
-
-#include "frc/geometry/Pose2d.h"
-
-namespace frc {
-
-/**
- * Define a unit for curvature.
- */
-using curvature_t = units::unit_t<
-    units::compound_unit<units::radian, units::inverse<units::meter>>>;
-
-/**
- * Represents a two-dimensional parametric spline that interpolates between two
- * points.
- *
- * @tparam Degree The degree of the spline.
- */
-template <int Degree>
-class Spline {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  Spline() = default;
-
-  Spline(const Spline&) = default;
-  Spline& operator=(const Spline&) = default;
-
-  Spline(Spline&&) = default;
-  Spline& operator=(Spline&&) = default;
-
-  virtual ~Spline() = default;
-
-  /**
-   * Represents a control vector for a spline.
-   *
-   * Each element in each array represents the value of the derivative at the
-   * index. For example, the value of x[2] is the second derivative in the x
-   * dimension.
-   */
-  struct ControlVector {
-    std::array<double, (Degree + 1) / 2> x;
-    std::array<double, (Degree + 1) / 2> y;
-  };
-
-  /**
-   * Gets the pose and curvature at some point t on the spline.
-   *
-   * @param t The point t
-   * @return The pose and curvature at that point.
-   */
-  PoseWithCurvature GetPoint(double t) const {
-    Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
-
-    // Populate the polynomial bases
-    for (int i = 0; i <= Degree; i++) {
-      polynomialBases(i) = std::pow(t, Degree - i);
-    }
-
-    // This simply multiplies by the coefficients. We need to divide out t some
-    // n number of times where n is the derivative we want to take.
-    Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
-
-    double dx, dy, ddx, ddy;
-
-    // If t = 0, all other terms in the equation cancel out to zero. We can use
-    // the last x^0 term in the equation.
-    if (t == 0.0) {
-      dx = Coefficients()(2, Degree - 1);
-      dy = Coefficients()(3, Degree - 1);
-      ddx = Coefficients()(4, Degree - 2);
-      ddy = Coefficients()(5, Degree - 2);
-    } else {
-      // Divide out t for first derivative.
-      dx = combined(2) / t;
-      dy = combined(3) / t;
-
-      // Divide out t for second derivative.
-      ddx = combined(4) / t / t;
-      ddy = combined(5) / t / t;
-    }
-
-    // Find the curvature.
-    const auto curvature =
-        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
-
-    return {
-        {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
-        curvature_t(curvature)};
-  }
-
- protected:
-  /**
-   * Returns the coefficients of the spline.
-   *
-   * @return The coefficients of the spline.
-   */
-  virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
-
-  /**
-   * Converts a Translation2d into a vector that is compatible with Eigen.
-   *
-   * @param translation The Translation2d to convert.
-   * @return The vector.
-   */
-  static Eigen::Vector2d ToVector(const Translation2d& translation) {
-    return (Eigen::Vector2d() << translation.X().to<double>(),
-            translation.Y().to<double>())
-        .finished();
-  }
-
-  /**
-   * Converts an Eigen vector into a Translation2d.
-   *
-   * @param vector The vector to convert.
-   * @return The Translation2d.
-   */
-  static Translation2d FromVector(const Eigen::Vector2d& vector) {
-    return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
-  }
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/spline/SplineHelper.h

diff --git a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h
deleted file mode 100644
index 4ed1cf5..0000000
--- a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h
+++ /dev/null
@@ -1,113 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <array>
-#include <utility>
-#include <vector>
-
-#include "frc/spline/CubicHermiteSpline.h"
-#include "frc/spline/QuinticHermiteSpline.h"
-
-namespace frc {
-/**
- * Helper class that is used to generate cubic and quintic splines from user
- * provided waypoints.
- */
-class SplineHelper {
- public:
-  /**
-   * Returns 2 cubic control vectors from a set of exterior waypoints and
-   * interior translations.
-   *
-   * @param start             The starting pose.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending pose.
-   * @return 2 cubic control vectors.
-   */
-  static std::array<Spline<3>::ControlVector, 2>
-  CubicControlVectorsFromWaypoints(
-      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-      const Pose2d& end);
-
-  /**
-   * Returns quintic control vectors from a set of waypoints.
-   *
-   * @param waypoints The waypoints
-   * @return List of control vectors
-   */
-  static std::vector<Spline<5>::ControlVector>
-  QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
-
-  /**
-   * Returns a set of cubic splines corresponding to the provided control
-   * vectors. The user is free to set the direction of the start and end
-   * point. The directions for the middle waypoints are determined
-   * automatically to ensure continuous curvature throughout the path.
-   *
-   * The derivation for the algorithm used can be found here:
-   * <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
-   *
-   * @param start The starting control vector.
-   * @param waypoints The middle waypoints. This can be left blank if you
-   * only wish to create a path with two waypoints.
-   * @param end The ending control vector.
-   *
-   * @return A vector of cubic hermite splines that interpolate through the
-   * provided waypoints.
-   */
-  static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
-      const Spline<3>::ControlVector& start,
-      std::vector<Translation2d> waypoints,
-      const Spline<3>::ControlVector& end);
-
-  /**
-   * Returns a set of quintic splines corresponding to the provided control
-   * vectors. The user is free to set the direction of all waypoints. Continuous
-   * curvature is guaranteed throughout the path.
-   *
-   * @param controlVectors The control vectors.
-   * @return A vector of quintic hermite splines that interpolate through the
-   * provided waypoints.
-   */
-  static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
-      const std::vector<Spline<5>::ControlVector>& controlVectors);
-
- private:
-  static Spline<3>::ControlVector CubicControlVector(double scalar,
-                                                     const Pose2d& point) {
-    return {
-        {point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
-        {point.Translation().Y().to<double>(),
-         scalar * point.Rotation().Sin()}};
-  }
-
-  static Spline<5>::ControlVector QuinticControlVector(double scalar,
-                                                       const Pose2d& point) {
-    return {{point.Translation().X().to<double>(),
-             scalar * point.Rotation().Cos(), 0.0},
-            {point.Translation().Y().to<double>(),
-             scalar * point.Rotation().Sin(), 0.0}};
-  }
-
-  /**
-   * Thomas algorithm for solving tridiagonal systems Af = d.
-   *
-   * @param a the values of A above the diagonal
-   * @param b the values of A on the diagonal
-   * @param c the values of A below the diagonal
-   * @param d the vector on the rhs
-   * @param solutionVector the unknown (solution) vector, modified in-place
-   */
-  static void ThomasAlgorithm(const std::vector<double>& a,
-                              const std::vector<double>& b,
-                              const std::vector<double>& c,
-                              const std::vector<double>& d,
-                              std::vector<double>* solutionVector);
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h

diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
deleted file mode 100644
index d3a5e6e..0000000
--- a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <frc/spline/Spline.h>
-
-#include <stack>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include <units/units.h>
-#include <wpi/Twine.h>
-
-namespace frc {
-
-/**
- * Class used to parameterize a spline by its arc length.
- */
-class SplineParameterizer {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  struct MalformedSplineException : public std::runtime_error {
-    explicit MalformedSplineException(const char* what_arg)
-        : runtime_error(what_arg) {}
-  };
-
-  /**
-   * Parameterizes the spline. This method breaks up the spline into various
-   * arcs until their dx, dy, and dtheta are within specific tolerances.
-   *
-   * @param spline The spline to parameterize.
-   * @param t0 Starting internal spline parameter. It is recommended to leave
-   * this as default.
-   * @param t1 Ending internal spline parameter. It is recommended to leave this
-   * as default.
-   *
-   * @return A vector of poses and curvatures that represents various points on
-   * the spline.
-   */
-  template <int Dim>
-  static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
-                                                     double t0 = 0.0,
-                                                     double t1 = 1.0) {
-    std::vector<PoseWithCurvature> splinePoints;
-
-    // The parameterization does not add the initial point. Let's add that.
-    splinePoints.push_back(spline.GetPoint(t0));
-
-    // We use an "explicit stack" to simulate recursion, instead of a recursive
-    // function call This give us greater control, instead of a stack overflow
-    std::stack<StackContents> stack;
-    stack.emplace(StackContents{t0, t1});
-
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
-    int iterations = 0;
-
-    while (!stack.empty()) {
-      current = stack.top();
-      stack.pop();
-      start = spline.GetPoint(current.t0);
-      end = spline.GetPoint(current.t1);
-
-      const auto twist = start.first.Log(end.first);
-
-      if (units::math::abs(twist.dy) > kMaxDy ||
-          units::math::abs(twist.dx) > kMaxDx ||
-          units::math::abs(twist.dtheta) > kMaxDtheta) {
-        stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
-        stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
-      } else {
-        splinePoints.push_back(spline.GetPoint(current.t1));
-      }
-
-      if (iterations++ >= kMaxIterations) {
-        throw MalformedSplineException(
-            "Could not parameterize a malformed spline. "
-            "This means that you probably had two or more adjacent "
-            "waypoints that were very close together with headings "
-            "in opposing directions.");
-      }
-    }
-
-    return splinePoints;
-  }
-
- private:
-  // Constraints for spline parameterization.
-  static constexpr units::meter_t kMaxDx = 5_in;
-  static constexpr units::meter_t kMaxDy = 0.05_in;
-  static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
-
-  struct StackContents {
-    double t0;
-    double t1;
-  };
-
-  /**
-   * A malformed spline does not actually explode the LIFO stack size. Instead,
-   * the stack size stays at a relatively small number (e.g. 30) and never
-   * decreases. Because of this, we must count iterations. Even long, complex
-   * paths don't usually go over 300 iterations, so hitting this maximum should
-   * definitely indicate something has gone wrong.
-   */
-  static constexpr int kMaxIterations = 5000;
-
-  friend class CubicHermiteSplineTest;
-  friend class QuinticHermiteSplineTest;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/Trajectory.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
deleted file mode 100644
index 13e32d6..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Transform2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * Define a unit for curvature.
- */
-using curvature_t = units::unit_t<
-    units::compound_unit<units::radian, units::inverse<units::meter>>>;
-
-/**
- * Represents a time-parameterized trajectory. The trajectory contains of
- * various States that represent the pose, curvature, time elapsed, velocity,
- * and acceleration at that point.
- */
-class Trajectory {
- public:
-  /**
-   * Represents one point on the trajectory.
-   */
-  struct State {
-    // The time elapsed since the beginning of the trajectory.
-    units::second_t t = 0_s;
-
-    // The speed at that point of the trajectory.
-    units::meters_per_second_t velocity = 0_mps;
-
-    // The acceleration at that point of the trajectory.
-    units::meters_per_second_squared_t acceleration = 0_mps_sq;
-
-    // The pose at that point of the trajectory.
-    Pose2d pose;
-
-    // The curvature at that point of the trajectory.
-    curvature_t curvature{0.0};
-
-    /**
-     * Checks equality between this State and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are equal.
-     */
-    bool operator==(const State& other) const;
-
-    /**
-     * Checks inequality between this State and another object.
-     *
-     * @param other The other object.
-     * @return Whether the two objects are not equal.
-     */
-    bool operator!=(const State& other) const;
-
-    /**
-     * Interpolates between two States.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    State Interpolate(State endValue, double i) const;
-  };
-
-  Trajectory() = default;
-
-  /**
-   * Constructs a trajectory from a vector of states.
-   */
-  explicit Trajectory(const std::vector<State>& states);
-
-  /**
-   * Returns the overall duration of the trajectory.
-   * @return The duration of the trajectory.
-   */
-  units::second_t TotalTime() const { return m_totalTime; }
-
-  /**
-   * Return the states of the trajectory.
-   * @return The states of the trajectory.
-   */
-  const std::vector<State>& States() const { return m_states; }
-
-  /**
-   * Sample the trajectory at a point in time.
-   *
-   * @param t The point in time since the beginning of the trajectory to sample.
-   * @return The state at that point in time.
-   */
-  State Sample(units::second_t t) const;
-
-  /**
-   * Transforms all poses in the trajectory by the given transform. This is
-   * useful for converting a robot-relative trajectory into a field-relative
-   * trajectory. This works with respect to the first pose in the trajectory.
-   *
-   * @param transform The transform to transform the trajectory by.
-   * @return The transformed trajectory.
-   */
-  Trajectory TransformBy(const Transform2d& transform);
-
-  /**
-   * Transforms all poses in the trajectory so that they are relative to the
-   * given pose. This is useful for converting a field-relative trajectory
-   * into a robot-relative trajectory.
-   *
-   * @param pose The pose that is the origin of the coordinate frame that
-   *             the current trajectory will be transformed into.
-   * @return The transformed trajectory.
-   */
-  Trajectory RelativeTo(const Pose2d& pose);
-
-  /**
-   * Returns the initial pose of the trajectory.
-   *
-   * @return The initial pose of the trajectory.
-   */
-  Pose2d InitialPose() const { return Sample(0_s).pose; }
-
- private:
-  std::vector<State> m_states;
-  units::second_t m_totalTime;
-
-  /**
-   * 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;
-  }
-};
-
-void to_json(wpi::json& json, const Trajectory::State& state);
-
-void from_json(const wpi::json& json, Trajectory::State& state);
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
deleted file mode 100644
index 36118a0..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ /dev/null
@@ -1,165 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
-#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-/**
- * Represents the configuration for generating a trajectory. This class stores
- * the start velocity, end velocity, max velocity, max acceleration, custom
- * constraints, and the reversed flag.
- *
- * The class must be constructed with a max velocity and max acceleration.
- * The other parameters (start velocity, end velocity, constraints, reversed)
- * have been defaulted to reasonable values (0, 0, {}, false). These values can
- * be changed via the SetXXX methods.
- */
-class TrajectoryConfig {
- public:
-  /**
-   * Constructs a config object.
-   * @param maxVelocity The max velocity of the trajectory.
-   * @param maxAcceleration The max acceleration of the trajectory.
-   */
-  TrajectoryConfig(units::meters_per_second_t maxVelocity,
-                   units::meters_per_second_squared_t maxAcceleration)
-      : m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
-
-  TrajectoryConfig(const TrajectoryConfig&) = delete;
-  TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
-
-  TrajectoryConfig(TrajectoryConfig&&) = default;
-  TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
-
-  /**
-   * Sets the start velocity of the trajectory.
-   * @param startVelocity The start velocity of the trajectory.
-   */
-  void SetStartVelocity(units::meters_per_second_t startVelocity) {
-    m_startVelocity = startVelocity;
-  }
-
-  /**
-   * Sets the end velocity of the trajectory.
-   * @param endVelocity The end velocity of the trajectory.
-   */
-  void SetEndVelocity(units::meters_per_second_t endVelocity) {
-    m_endVelocity = endVelocity;
-  }
-
-  /**
-   * Sets the reversed flag of the trajectory.
-   * @param reversed Whether the trajectory should be reversed or not.
-   */
-  void SetReversed(bool reversed) { m_reversed = reversed; }
-
-  /**
-   * Adds a user-defined constraint to the trajectory.
-   * @param constraint The user-defined constraint.
-   */
-  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                     TrajectoryConstraint, Constraint>>>
-  void AddConstraint(Constraint constraint) {
-    m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
-  }
-
-  /**
-   * Adds a differential drive kinematics constraint to ensure that
-   * no wheel velocity of a differential drive goes above the max velocity.
-   *
-   * @param kinematics The differential drive kinematics.
-   */
-  void SetKinematics(const DifferentialDriveKinematics& kinematics) {
-    AddConstraint(
-        DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
-  }
-
-  /**
-   * Adds a mecanum drive kinematics constraint to ensure that
-   * no wheel velocity of a mecanum drive goes above the max velocity.
-   *
-   * @param kinematics The mecanum drive kinematics.
-   */
-  void SetKinematics(MecanumDriveKinematics kinematics) {
-    AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
-  }
-
-  /**
-   * Adds a swerve drive kinematics constraint to ensure that
-   * no wheel velocity of a swerve drive goes above the max velocity.
-   *
-   * @param kinematics The swerve drive kinematics.
-   */
-  template <size_t NumModules>
-  void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
-    AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
-  }
-
-  /**
-   * Returns the starting velocity of the trajectory.
-   * @return The starting velocity of the trajectory.
-   */
-  units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
-
-  /**
-   * Returns the ending velocity of the trajectory.
-   * @return The ending velocity of the trajectory.
-   */
-  units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
-
-  /**
-   * Returns the maximum velocity of the trajectory.
-   * @return The maximum velocity of the trajectory.
-   */
-  units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
-
-  /**
-   * Returns the maximum acceleration of the trajectory.
-   * @return The maximum acceleration of the trajectory.
-   */
-  units::meters_per_second_squared_t MaxAcceleration() const {
-    return m_maxAcceleration;
-  }
-
-  /**
-   * Returns the user-defined constraints of the trajectory.
-   * @return The user-defined constraints of the trajectory.
-   */
-  const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
-      const {
-    return m_constraints;
-  }
-
-  /**
-   * Returns whether the trajectory is reversed or not.
-   * @return whether the trajectory is reversed or not.
-   */
-  bool IsReversed() const { return m_reversed; }
-
- private:
-  units::meters_per_second_t m_startVelocity = 0_mps;
-  units::meters_per_second_t m_endVelocity = 0_mps;
-  units::meters_per_second_t m_maxVelocity;
-  units::meters_per_second_squared_t m_maxAcceleration;
-  std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
-  bool m_reversed = false;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
deleted file mode 100644
index 40e7e7b..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include "frc/spline/SplineParameterizer.h"
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryConfig.h"
-#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-/**
- * Helper class used to generate trajectories with various constraints.
- */
-class TrajectoryGenerator {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  /**
-   * Generates a trajectory from the given control vectors and config. This
-   * method uses clamped cubic splines -- a method in which the exterior control
-   * vectors and interior waypoints are provided. The headings are automatically
-   * determined at the interior points to ensure continuous curvature.
-   *
-   * @param initial           The initial control vector.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending control vector.
-   * @param config            The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(
-      Spline<3>::ControlVector initial,
-      const std::vector<Translation2d>& interiorWaypoints,
-      Spline<3>::ControlVector end, const TrajectoryConfig& config);
-
-  /**
-   * Generates a trajectory from the given waypoints and config. This method
-   * uses clamped cubic splines -- a method in which the initial pose, final
-   * pose, and interior waypoints are provided.  The headings are automatically
-   * determined at the interior points to ensure continuous curvature.
-   *
-   * @param start             The starting pose.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending pose.
-   * @param config            The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(
-      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-      const Pose2d& end, const TrajectoryConfig& config);
-
-  /**
-   * Generates a trajectory from the given quintic control vectors and config.
-   * This method uses quintic hermite splines -- therefore, all points must be
-   * represented by control vectors. Continuous curvature is guaranteed in this
-   * method.
-   *
-   * @param controlVectors List of quintic control vectors.
-   * @param config         The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(
-      std::vector<Spline<5>::ControlVector> controlVectors,
-      const TrajectoryConfig& config);
-
-  /**
-   * Generates a trajectory from the given waypoints and config. This method
-   * uses quintic hermite splines -- therefore, all points must be represented
-   * by Pose2d objects. Continuous curvature is guaranteed in this method.
-   *
-   * @param waypoints List of waypoints..
-   * @param config    The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
-                                       const TrajectoryConfig& config);
-
-  /**
-   * Generate spline points from a vector of splines by parameterizing the
-   * splines.
-   *
-   * @param splines The splines to parameterize.
-   *
-   * @return The spline points for use in time parameterization of a trajectory.
-   */
-  template <typename Spline>
-  static std::vector<PoseWithCurvature> SplinePointsFromSplines(
-      const std::vector<Spline>& splines) {
-    // Create the vector of spline points.
-    std::vector<PoseWithCurvature> splinePoints;
-
-    // Add the first point to the vector.
-    splinePoints.push_back(splines.front().GetPoint(0.0));
-
-    // Iterate through the vector and parameterize each spline, adding the
-    // parameterized points to the final vector.
-    for (auto&& spline : splines) {
-      auto points = SplineParameterizer::Parameterize(spline);
-      // Append the array of poses to the vector. We are removing the first
-      // point because it's a duplicate of the last point from the previous
-      // spline.
-      splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
-                          std::end(points));
-    }
-    return splinePoints;
-  }
-
- private:
-  static const Trajectory kDoNothingTrajectory;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
deleted file mode 100644
index b8ea8da..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-/**
- * Class used to parameterize a trajectory by time.
- */
-class TrajectoryParameterizer {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  /**
-   * Parameterize the trajectory by time. This is where the velocity profile is
-   * generated.
-   *
-   * The derivation of the algorithm used can be found here:
-   * <http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf>
-   *
-   * @param points Reference to the spline points.
-   * @param constraints A vector of various velocity and acceleration
-   * constraints.
-   * @param startVelocity The start velocity for the trajectory.
-   * @param endVelocity The end velocity for the trajectory.
-   * @param maxVelocity The max velocity for the trajectory.
-   * @param maxAcceleration The max acceleration for the trajectory.
-   * @param reversed Whether the robot should move backwards. Note that the
-   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
-   *
-   * @return The trajectory.
-   */
-  static Trajectory TimeParameterizeTrajectory(
-      const std::vector<PoseWithCurvature>& points,
-      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-      units::meters_per_second_t startVelocity,
-      units::meters_per_second_t endVelocity,
-      units::meters_per_second_t maxVelocity,
-      units::meters_per_second_squared_t maxAcceleration, bool reversed);
-
- private:
-  constexpr static double kEpsilon = 1E-6;
-
-  /**
-   * Represents a constrained state that is used when time parameterizing a
-   * trajectory. Each state has the pose, curvature, distance from the start of
-   * the trajectory, max velocity, min acceleration and max acceleration.
-   */
-  struct ConstrainedState {
-    PoseWithCurvature pose = {Pose2d(), curvature_t(0.0)};
-    units::meter_t distance = 0_m;
-    units::meters_per_second_t maxVelocity = 0_mps;
-    units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
-    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
-  };
-
-  /**
-   * Enforces acceleration limits as defined by the constraints. This function
-   * is used when time parameterizing a trajectory.
-   *
-   * @param reverse Whether the robot is traveling backwards.
-   * @param constraints A vector of the user-defined velocity and acceleration
-   * constraints.
-   * @param state Pointer to the constrained state that we are operating on.
-   * This is mutated in place.
-   */
-  static void EnforceAccelerationLimits(
-      bool reverse,
-      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-      ConstrainedState* state);
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
deleted file mode 100644
index 700ed9c..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-
-#include "frc/trajectory/Trajectory.h"
-
-namespace frc {
-class TrajectoryUtil {
- public:
-  TrajectoryUtil() = delete;
-
-  /**
-   * Exports a Trajectory to a PathWeaver-style JSON file.
-   *
-   * @param trajectory the trajectory to export
-   * @param path the path of the file to export to
-   *
-   * @return The interpolated state.
-   */
-  static void ToPathweaverJson(const Trajectory& trajectory,
-                               const wpi::Twine& path);
-  /**
-   * Imports a Trajectory from a PathWeaver-style JSON file.
-   *
-   * @param path The path of the json file to import from.
-   *
-   * @return The trajectory represented by the file.
-   */
-  static Trajectory FromPathweaverJson(const wpi::Twine& path);
-
-  /**
-     * Deserializes a Trajectory from PathWeaver-style JSON.
-
-     * @param json the string containing the serialized JSON
-
-     * @return the trajectory represented by the JSON
-     */
-  static std::string SerializeTrajectory(const Trajectory& trajectory);
-
-  /**
-   * Serializes a Trajectory to PathWeaver-style JSON.
-
-   * @param trajectory the trajectory to export
-
-   * @return the string containing the serialized JSON
-   */
-  static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
deleted file mode 100644
index 575088e..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ /dev/null
@@ -1,157 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <hal/FRCUsageReporting.h>
-#include <units/units.h>
-
-namespace frc {
-
-/**
- * A trapezoid-shaped velocity profile.
- *
- * While this class can be used for a profiled movement from start to finish,
- * the intended usage is to filter a reference's dynamics based on trapezoidal
- * velocity constraints. To compute the reference obeying this constraint, do
- * the following.
- *
- * Initialization:
- * @code{.cpp}
- * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
- * double previousProfiledReference = initialReference;
- * @endcode
- *
- * Run on update:
- * @code{.cpp}
- * TrapezoidalMotionProfile profile{constraints, unprofiledReference,
- *                                  previousProfiledReference};
- * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
- * @endcode
- *
- * where `unprofiledReference` is free to change between calls. Note that when
- * the unprofiled reference is within the constraints, `Calculate()` returns the
- * unprofiled reference unchanged.
- *
- * Otherwise, a timer can be started to provide monotonic values for
- * `Calculate()` and to determine when the profile has completed via
- * `IsFinished()`.
- */
-template <class Distance>
-class TrapezoidProfile {
-  using Distance_t = units::unit_t<Distance>;
-  using Velocity =
-      units::compound_unit<Distance, units::inverse<units::seconds>>;
-  using Velocity_t = units::unit_t<Velocity>;
-  using Acceleration =
-      units::compound_unit<Velocity, units::inverse<units::seconds>>;
-  using Acceleration_t = units::unit_t<Acceleration>;
-
- public:
-  class Constraints {
-   public:
-    Constraints() {
-      HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
-    }
-    Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
-        : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
-      HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
-    }
-    Velocity_t maxVelocity{0};
-    Acceleration_t maxAcceleration{0};
-  };
-
-  class State {
-   public:
-    Distance_t position{0};
-    Velocity_t velocity{0};
-    bool operator==(const State& rhs) const {
-      return position == rhs.position && velocity == rhs.velocity;
-    }
-    bool operator!=(const State& rhs) const { return !(*this == rhs); }
-  };
-
-  /**
-   * Construct a TrapezoidProfile.
-   *
-   * @param constraints The constraints on the profile, like maximum velocity.
-   * @param goal        The desired state when the profile is complete.
-   * @param initial     The initial state (usually the current state).
-   */
-  TrapezoidProfile(Constraints constraints, State goal,
-                   State initial = State{Distance_t(0), Velocity_t(0)});
-
-  TrapezoidProfile(const TrapezoidProfile&) = default;
-  TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
-  TrapezoidProfile(TrapezoidProfile&&) = default;
-  TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
-
-  /**
-   * Calculate the correct position and velocity for the profile at a time t
-   * where the beginning of the profile was at time t = 0.
-   *
-   * @param t The time since the beginning of the profile.
-   */
-  State Calculate(units::second_t t) const;
-
-  /**
-   * Returns the time left until a target distance in the profile is reached.
-   *
-   * @param target The target distance.
-   */
-  units::second_t TimeLeftUntil(Distance_t target) const;
-
-  /**
-   * Returns the total time the profile takes to reach the goal.
-   */
-  units::second_t TotalTime() const { return m_endDeccel; }
-
-  /**
-   * Returns true if the profile has reached the goal.
-   *
-   * The profile has reached the goal if the time since the profile started
-   * has exceeded the profile's total time.
-   *
-   * @param t The time since the beginning of the profile.
-   */
-  bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
-
- private:
-  /**
-   * Returns true if the profile inverted.
-   *
-   * The profile is inverted if goal position is less than the initial position.
-   *
-   * @param initial The initial state (usually the current state).
-   * @param goal    The desired state when the profile is complete.
-   */
-  static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
-    return initial.position > goal.position;
-  }
-
-  // Flip the sign of the velocity and position if the profile is inverted
-  State Direct(const State& in) const {
-    State result = in;
-    result.position *= m_direction;
-    result.velocity *= m_direction;
-    return result;
-  }
-
-  // The direction of the profile, either 1 for forwards or -1 for inverted
-  int m_direction;
-
-  Constraints m_constraints;
-  State m_initial;
-  State m_goal;
-
-  units::second_t m_endAccel;
-  units::second_t m_endFullSpeed;
-  units::second_t m_endDeccel;
-};
-}  // namespace frc
-
-#include "TrapezoidProfile.inc"

wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc

diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
deleted file mode 100644
index 50f232d..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ /dev/null
@@ -1,163 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <algorithm>
-
-namespace frc {
-template <class Distance>
-TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
-                                             State goal, State initial)
-    : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
-      m_constraints(constraints),
-      m_initial(Direct(initial)),
-      m_goal(Direct(goal)) {
-  if (m_initial.velocity > m_constraints.maxVelocity) {
-    m_initial.velocity = m_constraints.maxVelocity;
-  }
-
-  // Deal with a possibly truncated motion profile (with nonzero initial or
-  // final velocity) by calculating the parameters as if the profile began and
-  // ended at zero velocity
-  units::second_t cutoffBegin =
-      m_initial.velocity / m_constraints.maxAcceleration;
-  Distance_t cutoffDistBegin =
-      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
-
-  units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
-  Distance_t cutoffDistEnd =
-      cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
-
-  // Now we can calculate the parameters as if it was a full trapezoid instead
-  // of a truncated one
-
-  Distance_t fullTrapezoidDist =
-      cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
-  units::second_t accelerationTime =
-      m_constraints.maxVelocity / m_constraints.maxAcceleration;
-
-  Distance_t fullSpeedDist =
-      fullTrapezoidDist -
-      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
-
-  // Handle the case where the profile never reaches full speed
-  if (fullSpeedDist < Distance_t(0)) {
-    accelerationTime =
-        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
-    fullSpeedDist = Distance_t(0);
-  }
-
-  m_endAccel = accelerationTime - cutoffBegin;
-  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
-  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
-}
-
-template <class Distance>
-typename TrapezoidProfile<Distance>::State
-TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
-  State result = m_initial;
-
-  if (t < m_endAccel) {
-    result.velocity += t * m_constraints.maxAcceleration;
-    result.position +=
-        (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
-  } else if (t < m_endFullSpeed) {
-    result.velocity = m_constraints.maxVelocity;
-    result.position += (m_initial.velocity +
-                        m_endAccel * m_constraints.maxAcceleration / 2.0) *
-                           m_endAccel +
-                       m_constraints.maxVelocity * (t - m_endAccel);
-  } else if (t <= m_endDeccel) {
-    result.velocity =
-        m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
-    units::second_t timeLeft = m_endDeccel - t;
-    result.position =
-        m_goal.position -
-        (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
-            timeLeft;
-  } else {
-    result = m_goal;
-  }
-
-  return Direct(result);
-}
-
-template <class Distance>
-units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
-    Distance_t target) const {
-  Distance_t position = m_initial.position * m_direction;
-  Velocity_t velocity = m_initial.velocity * m_direction;
-
-  units::second_t endAccel = m_endAccel * m_direction;
-  units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
-
-  if (target < position) {
-    endAccel *= -1.0;
-    endFullSpeed *= -1.0;
-    velocity *= -1.0;
-  }
-
-  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;
-
-  Distance_t distToTarget = units::math::abs(target - position);
-
-  if (distToTarget < Distance_t(1e-6)) {
-    return 0_s;
-  }
-
-  Distance_t accelDist =
-      velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
-
-  Velocity_t deccelVelocity;
-  if (endAccel > 0_s) {
-    deccelVelocity = units::math::sqrt(
-        units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
-  } else {
-    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;
-
-  if (accelDist > distToTarget) {
-    accelDist = distToTarget;
-    fullSpeedDist = Distance_t(0);
-    deccelDist = Distance_t(0);
-  } else if (accelDist + fullSpeedDist > distToTarget) {
-    fullSpeedDist = distToTarget - accelDist;
-    deccelDist = Distance_t(0);
-  } else {
-    deccelDist = distToTarget - fullSpeedDist - accelDist;
-  }
-
-  units::second_t accelTime =
-      (-velocity + units::math::sqrt(units::math::abs(
-                       velocity * velocity + 2 * acceleration * accelDist))) /
-      acceleration;
-
-  units::second_t deccelTime =
-      (-deccelVelocity +
-       units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
-                                          2 * decceleration * deccelDist))) /
-      decceleration;
-
-  units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
-
-  return accelTime + fullSpeedTime + deccelTime;
-}
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
deleted file mode 100644
index de25738..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-
-/**
- * A constraint on the maximum absolute centripetal acceleration allowed when
- * traversing a trajectory. The centripetal acceleration of a robot is defined
- * as the velocity squared divided by the radius of curvature.
- *
- * Effectively, limiting the maximum centripetal acceleration will cause the
- * robot to slow down around tight turns, making it easier to track trajectories
- * with sharp turns.
- */
-class CentripetalAccelerationConstraint : public TrajectoryConstraint {
- public:
-  explicit CentripetalAccelerationConstraint(
-      units::meters_per_second_squared_t maxCentripetalAcceleration);
-
-  units::meters_per_second_t MaxVelocity(
-      const Pose2d& pose, curvature_t curvature,
-      units::meters_per_second_t velocity) override;
-
-  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
-                            units::meters_per_second_t speed) override;
-
- private:
-  units::meters_per_second_squared_t m_maxCentripetalAcceleration;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
deleted file mode 100644
index 6259c96..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-/**
- * A class that enforces constraints on the differential drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for both sides of the drivetrain stay below a certain
- * limit.
- */
-namespace frc {
-class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
- public:
-  DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics,
-                                        units::meters_per_second_t maxSpeed);
-
-  units::meters_per_second_t MaxVelocity(
-      const Pose2d& pose, curvature_t curvature,
-      units::meters_per_second_t velocity) override;
-
-  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
-                            units::meters_per_second_t speed) override;
-
- private:
-  DifferentialDriveKinematics m_kinematics;
-  units::meters_per_second_t m_maxSpeed;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
deleted file mode 100644
index 50ace85..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/controller/SimpleMotorFeedforward.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-/**
- * A class that enforces constraints on differential drive voltage expenditure
- * based on the motor dynamics and the drive kinematics.  Ensures that the
- * acceleration of any wheel of the robot while following the trajectory is
- * never higher than what can be achieved with the given maximum voltage.
- */
-class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
- public:
-  /**
-   * Creates a new DifferentialDriveVoltageConstraint.
-   *
-   * @param feedforward A feedforward component describing the behavior of the
-   * drive.
-   * @param kinematics  A kinematics component describing the drive geometry.
-   * @param maxVoltage  The maximum voltage available to the motors while
-   * following the path. Should be somewhat less than the nominal battery
-   * voltage (12V) to account for "voltage sag" due to current draw.
-   */
-  DifferentialDriveVoltageConstraint(
-      SimpleMotorFeedforward<units::meter> feedforward,
-      DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
-
-  units::meters_per_second_t MaxVelocity(
-      const Pose2d& pose, curvature_t curvature,
-      units::meters_per_second_t velocity) override;
-
-  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
-                            units::meters_per_second_t speed) override;
-
- private:
-  SimpleMotorFeedforward<units::meter> m_feedforward;
-  DifferentialDriveKinematics m_kinematics;
-  units::volt_t m_maxVoltage;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
deleted file mode 100644
index 53e3953..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <cmath>
-
-#include <units/units.h>
-
-#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-/**
- * A class that enforces constraints on the differential drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for both sides of the drivetrain stay below a certain
- * limit.
- */
-namespace frc {
-class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
- public:
-  MecanumDriveKinematicsConstraint(MecanumDriveKinematics kinematics,
-                                   units::meters_per_second_t maxSpeed);
-
-  units::meters_per_second_t MaxVelocity(
-      const Pose2d& pose, curvature_t curvature,
-      units::meters_per_second_t velocity) override;
-
-  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
-                            units::meters_per_second_t speed) override;
-
- private:
-  MecanumDriveKinematics m_kinematics;
-  units::meters_per_second_t m_maxSpeed;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
deleted file mode 100644
index a8ad870..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <cmath>
-
-#include <units/units.h>
-
-#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-/**
- * A class that enforces constraints on the differential drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for both sides of the drivetrain stay below a certain
- * limit.
- */
-namespace frc {
-
-template <size_t NumModules>
-class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
- public:
-  SwerveDriveKinematicsConstraint(
-      frc::SwerveDriveKinematics<NumModules> kinematics,
-      units::meters_per_second_t maxSpeed);
-
-  units::meters_per_second_t MaxVelocity(
-      const Pose2d& pose, curvature_t curvature,
-      units::meters_per_second_t velocity) override;
-
-  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
-                            units::meters_per_second_t speed) override;
-
- private:
-  frc::SwerveDriveKinematics<NumModules> m_kinematics;
-  units::meters_per_second_t m_maxSpeed;
-};
-}  // namespace frc
-
-#include "SwerveDriveKinematicsConstraint.inc"

wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc

diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
deleted file mode 100644
index c3437d5..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-/**
- * A class that enforces constraints on the differential drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for both sides of the drivetrain stay below a certain
- * limit.
- */
-
-namespace frc {
-
-template <size_t NumModules>
-SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
-    frc::SwerveDriveKinematics<NumModules> kinematics,
-    units::meters_per_second_t maxSpeed)
-    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
-
-template <size_t NumModules>
-units::meters_per_second_t
-SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t velocity) {
-  auto xVelocity = velocity * pose.Rotation().Cos();
-  auto yVelocity = velocity * pose.Rotation().Sin();
-  auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
-      {xVelocity, yVelocity, velocity * curvature});
-  m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
-
-  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
-}
-
-template <size_t NumModules>
-TrajectoryConstraint::MinMax
-SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t speed) {
-  return {};
-}
-
-}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h

diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
deleted file mode 100644
index dcde8c4..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <limits>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/spline/Spline.h"
-
-namespace frc {
-/**
- * An interface for defining user-defined velocity and acceleration constraints
- * while generating trajectories.
- */
-class TrajectoryConstraint {
- public:
-  TrajectoryConstraint() = default;
-
-  TrajectoryConstraint(const TrajectoryConstraint&) = default;
-  TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default;
-
-  TrajectoryConstraint(TrajectoryConstraint&&) = default;
-  TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default;
-
-  virtual ~TrajectoryConstraint() = default;
-
-  /**
-   * Represents a minimum and maximum acceleration.
-   */
-  struct MinMax {
-    /**
-     * The minimum acceleration.
-     */
-    units::meters_per_second_squared_t minAcceleration{
-        -std::numeric_limits<double>::max()};
-
-    /**
-     * The maximum acceleration.
-     */
-    units::meters_per_second_squared_t maxAcceleration{
-        std::numeric_limits<double>::max()};
-  };
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param pose The pose at the current point in the trajectory.
-   * @param curvature The curvature at the current point in the trajectory.
-   * @param velocity The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   *
-   * @return The absolute maximum velocity.
-   */
-  virtual units::meters_per_second_t MaxVelocity(
-      const Pose2d& pose, curvature_t curvature,
-      units::meters_per_second_t velocity) = 0;
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param pose The pose at the current point in the trajectory.
-   * @param curvature The curvature at the current point in the trajectory.
-   * @param speed The speed at the current point in the trajectory.
-   *
-   * @return The min and max acceleration bounds.
-   */
-  virtual MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
-                                    units::meters_per_second_t speed) = 0;
-};
-}  // namespace frc

wpilibc/src/main/native/include/frc/util/Color.h

diff --git a/wpilibc/src/main/native/include/frc/util/Color.h b/wpilibc/src/main/native/include/frc/util/Color.h
index 2538d60..1321380 100644
--- a/wpilibc/src/main/native/include/frc/util/Color.h
+++ b/wpilibc/src/main/native/include/frc/util/Color.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -399,7 +399,7 @@
   /**
    * #20B2AA.
    */
-  static const Color kLightSeagGeen;
+  static const Color kLightSeaGreen;
 
   /**
    * #87CEFA.
@@ -414,7 +414,7 @@
   /**
    * #B0C4DE.
    */
-  static const Color kLightSteellue;
+  static const Color kLightSteelBlue;
 
   /**
    * #FFFFE0.
@@ -755,6 +755,42 @@
         green(roundAndClamp(g)),
         blue(roundAndClamp(b)) {}
 
+  /**
+   * Creates a Color from HSV values.
+   *
+   * @param h The h value [0-180]
+   * @param s The s value [0-255]
+   * @param v The v value [0-255]
+   * @return The color
+   */
+  static constexpr Color FromHSV(int h, int s, int v) {
+    if (s == 0) {
+      return {v / 255.0, v / 255.0, v / 255.0};
+    }
+
+    int region = h / 30;
+    int remainder = (h - (region * 30)) * 6;
+
+    int p = (v * (255 - s)) >> 8;
+    int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+    int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+    switch (region) {
+      case 0:
+        return Color(v / 255.0, t / 255.0, p / 255.0);
+      case 1:
+        return Color(q / 255.0, v / 255.0, p / 255.0);
+      case 2:
+        return Color(p / 255.0, v / 255.0, t / 255.0);
+      case 3:
+        return Color(p / 255.0, q / 255.0, v / 255.0);
+      case 4:
+        return Color(t / 255.0, p / 255.0, v / 255.0);
+      default:
+        return Color(v / 255.0, p / 255.0, q / 255.0);
+    }
+  }
+
   double red = 0.0;
   double green = 0.0;
   double blue = 0.0;
@@ -864,13 +900,13 @@
 inline constexpr Color Color::kLightGreen{0.5647059f, 0.93333334f, 0.5647059f};
 inline constexpr Color Color::kLightPink{1.0f, 0.7137255f, 0.75686276f};
 inline constexpr Color Color::kLightSalmon{1.0f, 0.627451f, 0.47843137f};
-inline constexpr Color Color::kLightSeagGeen{0.1254902f, 0.69803923f,
+inline constexpr Color Color::kLightSeaGreen{0.1254902f, 0.69803923f,
                                              0.6666667f};
 inline constexpr Color Color::kLightSkyBlue{0.5294118f, 0.80784315f,
                                             0.98039216f};
 inline constexpr Color Color::kLightSlateGray{0.46666667f, 0.53333336f, 0.6f};
-inline constexpr Color Color::kLightSteellue{0.6901961f, 0.76862746f,
-                                             0.87058824f};
+inline constexpr Color Color::kLightSteelBlue{0.6901961f, 0.76862746f,
+                                              0.87058824f};
 inline constexpr Color Color::kLightYellow{1.0f, 1.0f, 0.8784314f};
 inline constexpr Color Color::kLime{0.0f, 1.0f, 0.0f};
 inline constexpr Color Color::kLimeGreen{0.19607843f, 0.8039216f, 0.19607843f};

wpilibc/src/main/native/include/frc2/Timer.h

diff --git a/wpilibc/src/main/native/include/frc2/Timer.h b/wpilibc/src/main/native/include/frc2/Timer.h
index c1eeb16..8f943a6 100644
--- a/wpilibc/src/main/native/include/frc2/Timer.h
+++ b/wpilibc/src/main/native/include/frc2/Timer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,12 +7,9 @@
 
 #pragma once
 
-#include <units/units.h>
-#include <wpi/deprecated.h>
+#include <units/time.h>
 #include <wpi/mutex.h>
 
-#include "frc/Base.h"
-
 namespace frc2 {
 
 /**
@@ -75,7 +72,8 @@
    * Start the timer running.
    *
    * Just set the running flag to true indicating that all time requests should
-   * be relative to the system clock.
+   * be relative to the system clock. Note that this method is a no-op if the
+   * timer is already running.
    */
   void Start();
 
@@ -89,6 +87,14 @@
   void Stop();
 
   /**
+   * Check if the period specified has passed.
+   *
+   * @param seconds The period to check.
+   * @return        True if the period has passed.
+   */
+  bool HasElapsed(units::second_t period) const;
+
+  /**
    * Check if the period specified has passed and if it has, advance the start
    * time by that period. This is useful to decide if it's time to do periodic
    * work without drifting later by the time it took to get around to checking.
@@ -99,6 +105,16 @@
   bool HasPeriodPassed(units::second_t period);
 
   /**
+   * Check if the period specified has passed and if it has, advance the start
+   * time by that period. This is useful to decide if it's time to do periodic
+   * work without drifting later by the time it took to get around to checking.
+   *
+   * @param period The period to check for.
+   * @return       True if the period has passed.
+   */
+  bool AdvanceIfElapsed(units::second_t period);
+
+  /**
    * Return the FPGA system clock time in seconds.
    *
    * Return the time from the FPGA hardware clock in seconds since the FPGA
@@ -125,9 +141,6 @@
    */
   static units::second_t GetMatchTime();
 
-  // The time, in seconds, at which the 32-bit FPGA timestamp rolls over to 0
-  static const units::second_t kRolloverTime;
-
  private:
   units::second_t m_startTime = 0_s;
   units::second_t m_accumulatedTime = 0_s;

wpilibc/src/test/native/cpp/DriverStationTest.cpp

diff --git a/wpilibc/src/test/native/cpp/DriverStationTest.cpp b/wpilibc/src/test/native/cpp/DriverStationTest.cpp
new file mode 100644
index 0000000..3b8272f
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/DriverStationTest.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <string>
+#include <tuple>
+
+#include "frc/DriverStation.h"
+#include "frc/Joystick.h"
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+class IsJoystickConnectedParametersTests
+    : public ::testing::TestWithParam<std::tuple<int, int, int, bool>> {};
+
+TEST_P(IsJoystickConnectedParametersTests, IsJoystickConnected) {
+  frc::sim::DriverStationSim::SetJoystickAxisCount(1, std::get<0>(GetParam()));
+  frc::sim::DriverStationSim::SetJoystickButtonCount(1,
+                                                     std::get<1>(GetParam()));
+  frc::sim::DriverStationSim::SetJoystickPOVCount(1, std::get<2>(GetParam()));
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  ASSERT_EQ(std::get<3>(GetParam()),
+            frc::DriverStation::GetInstance().IsJoystickConnected(1));
+}
+
+INSTANTIATE_TEST_SUITE_P(IsConnectedTests, IsJoystickConnectedParametersTests,
+                         ::testing::Values(std::make_tuple(0, 0, 0, false),
+                                           std::make_tuple(1, 0, 0, true),
+                                           std::make_tuple(0, 1, 0, true),
+                                           std::make_tuple(0, 0, 1, true),
+                                           std::make_tuple(1, 1, 1, true),
+                                           std::make_tuple(4, 10, 1, true)));
+class JoystickConnectionWarningTests
+    : public ::testing::TestWithParam<
+          std::tuple<bool, bool, bool, std::string>> {};
+
+TEST_P(JoystickConnectionWarningTests, JoystickConnectionWarnings) {
+  // Capture all output to stderr.
+  ::testing::internal::CaptureStderr();
+
+  // Set FMS and Silence settings
+  frc::sim::DriverStationSim::SetFmsAttached(std::get<0>(GetParam()));
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::DriverStation::GetInstance().SilenceJoystickConnectionWarning(
+      std::get<1>(GetParam()));
+
+  // Create joystick and attempt to retrieve button.
+  frc::Joystick joystick(0);
+  joystick.GetRawButton(1);
+
+  frc::sim::StepTiming(1_s);
+  EXPECT_EQ(
+      frc::DriverStation::GetInstance().IsJoystickConnectionWarningSilenced(),
+      std::get<2>(GetParam()));
+  EXPECT_EQ(::testing::internal::GetCapturedStderr(), std::get<3>(GetParam()));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    DriverStation, JoystickConnectionWarningTests,
+    ::testing::Values(std::make_tuple(false, true, true, ""),
+                      std::make_tuple(false, false, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n"),
+                      std::make_tuple(true, true, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n"),
+                      std::make_tuple(true, false, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n")));

wpilibc/src/test/native/cpp/JoystickTest.cpp

diff --git a/wpilibc/src/test/native/cpp/JoystickTest.cpp b/wpilibc/src/test/native/cpp/JoystickTest.cpp
new file mode 100644
index 0000000..0480b76
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/JoystickTest.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Joystick.h"  // NOLINT(build/include_order)
+
+#include "frc/simulation/JoystickSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(JoystickTests, GetX) {
+  Joystick joy{1};
+  sim::JoystickSim joysim{joy};
+
+  joysim.SetX(0.25);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetX(), 0.25, 0.001);
+
+  joysim.SetX(0);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetX(), 0, 0.001);
+}
+
+TEST(JoystickTests, GetY) {
+  Joystick joy{1};
+  sim::JoystickSim joysim{joy};
+
+  joysim.SetY(0.25);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetY(), 0.25, 0.001);
+
+  joysim.SetY(0);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetY(), 0, 0.001);
+}

wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp

diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
deleted file mode 100644
index 0bb1002..0000000
--- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h"  // NOLINT(build/include_order)
-
-#include <cmath>
-#include <memory>
-#include <random>
-
-#include <units/units.h>
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-
-// Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr int32_t kMovAvgTaps = 6;
-
-enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
-
-std::ostream& operator<<(std::ostream& os,
-                         const LinearFilterNoiseTestType& type) {
-  switch (type) {
-    case TEST_SINGLE_POLE_IIR:
-      os << "LinearFilter SinglePoleIIR";
-      break;
-    case TEST_MOVAVG:
-      os << "LinearFilter MovingAverage";
-      break;
-  }
-
-  return os;
-}
-
-static double GetData(double t) {
-  return 100.0 * std::sin(2.0 * wpi::math::pi * t);
-}
-
-class LinearFilterNoiseTest
-    : public testing::TestWithParam<LinearFilterNoiseTestType> {
- protected:
-  std::unique_ptr<frc::LinearFilter<double>> m_filter;
-
-  void SetUp() override {
-    switch (GetParam()) {
-      case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                                     kFilterStep));
-        break;
-      }
-
-      case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
-        break;
-      }
-    }
-  }
-};
-
-/**
- * Test if the filter reduces the noise produced by a signal generator
- */
-TEST_P(LinearFilterNoiseTest, NoiseReduce) {
-  double noiseGenError = 0.0;
-  double filterError = 0.0;
-
-  std::random_device rd;
-  std::mt19937 gen{rd()};
-  std::normal_distribution<double> distr{0.0, 10.0};
-
-  for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
-    double theory = GetData(t.to<double>());
-    double noise = distr(gen);
-    filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
-    noiseGenError += std::abs(noise - theory);
-  }
-
-  RecordProperty("FilterError", filterError);
-
-  // The filter should have produced values closer to the theory
-  EXPECT_GT(noiseGenError, filterError)
-      << "Filter should have reduced noise accumulation but failed";
-}
-
-INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
-                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));

wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp

diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
deleted file mode 100644
index 8db91b3..0000000
--- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h"  // NOLINT(build/include_order)
-
-#include <cmath>
-#include <functional>
-#include <memory>
-#include <random>
-
-#include <units/units.h>
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-
-// Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
-static constexpr double kHighPassTimeConstant = 0.006631;
-static constexpr double kHighPassExpectedOutput = 10.074717;
-static constexpr int32_t kMovAvgTaps = 6;
-static constexpr double kMovAvgExpectedOutput = -10.191644;
-
-enum LinearFilterOutputTestType {
-  TEST_SINGLE_POLE_IIR,
-  TEST_HIGH_PASS,
-  TEST_MOVAVG,
-  TEST_PULSE
-};
-
-std::ostream& operator<<(std::ostream& os,
-                         const LinearFilterOutputTestType& type) {
-  switch (type) {
-    case TEST_SINGLE_POLE_IIR:
-      os << "LinearFilter SinglePoleIIR";
-      break;
-    case TEST_HIGH_PASS:
-      os << "LinearFilter HighPass";
-      break;
-    case TEST_MOVAVG:
-      os << "LinearFilter MovingAverage";
-      break;
-    case TEST_PULSE:
-      os << "LinearFilter Pulse";
-      break;
-  }
-
-  return os;
-}
-
-static double GetData(double t) {
-  return 100.0 * std::sin(2.0 * wpi::math::pi * t) +
-         20.0 * std::cos(50.0 * wpi::math::pi * t);
-}
-
-static double GetPulseData(double t) {
-  if (std::abs(t - 1.0) < 0.001) {
-    return 1.0;
-  } else {
-    return 0.0;
-  }
-}
-
-/**
- * A fixture that includes a consistent data source wrapped in a filter
- */
-class LinearFilterOutputTest
-    : public testing::TestWithParam<LinearFilterOutputTestType> {
- protected:
-  std::unique_ptr<frc::LinearFilter<double>> m_filter;
-  std::function<double(double)> m_data;
-  double m_expectedOutput = 0.0;
-
-  void SetUp() override {
-    switch (GetParam()) {
-      case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                                     kFilterStep));
-        m_data = GetData;
-        m_expectedOutput = kSinglePoleIIRExpectedOutput;
-        break;
-      }
-
-      case TEST_HIGH_PASS: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
-                                                kFilterStep));
-        m_data = GetData;
-        m_expectedOutput = kHighPassExpectedOutput;
-        break;
-      }
-
-      case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
-        m_data = GetData;
-        m_expectedOutput = kMovAvgExpectedOutput;
-        break;
-      }
-
-      case TEST_PULSE: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
-        m_data = GetPulseData;
-        m_expectedOutput = 0.0;
-        break;
-      }
-    }
-  }
-};
-
-/**
- * Test if the linear filters produce consistent output for a given data set.
- */
-TEST_P(LinearFilterOutputTest, Output) {
-  double filterOutput = 0.0;
-  for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
-    filterOutput = m_filter->Calculate(m_data(t.to<double>()));
-  }
-
-  RecordProperty("LinearFilterOutput", filterOutput);
-
-  EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
-      << "Filter output didn't match expected value";
-}
-
-INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
-                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
-                                         TEST_MOVAVG, TEST_PULSE));

wpilibc/src/test/native/cpp/MedianFilterTest.cpp

diff --git a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
deleted file mode 100644
index 7fe7d2e..0000000
--- a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/MedianFilter.h"
-#include "gtest/gtest.h"
-
-TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
-  frc::MedianFilter<double> filter{10};
-
-  filter.Calculate(3);
-  filter.Calculate(0);
-  filter.Calculate(4);
-
-  EXPECT_EQ(filter.Calculate(1000), 3.5);
-}
-
-TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
-  frc::MedianFilter<double> filter{10};
-
-  filter.Calculate(3);
-  filter.Calculate(0);
-  filter.Calculate(4);
-  filter.Calculate(7);
-
-  EXPECT_EQ(filter.Calculate(1000), 4);
-}
-
-TEST(MedianFilterTest, MedianFilterFullTestEven) {
-  frc::MedianFilter<double> filter{6};
-
-  filter.Calculate(3);
-  filter.Calculate(0);
-  filter.Calculate(0);
-  filter.Calculate(5);
-  filter.Calculate(4);
-  filter.Calculate(1000);
-
-  EXPECT_EQ(filter.Calculate(99), 4.5);
-}
-
-TEST(MedianFilterTest, MedianFilterFullTestOdd) {
-  frc::MedianFilter<double> filter{5};
-
-  filter.Calculate(3);
-  filter.Calculate(0);
-  filter.Calculate(5);
-  filter.Calculate(4);
-  filter.Calculate(1000);
-
-  EXPECT_EQ(filter.Calculate(99), 5);
-}

wpilibc/src/test/native/cpp/ScopedTracerTest.cpp

diff --git a/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp b/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp
new file mode 100644
index 0000000..da222de
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <string>
+#include <thread>
+
+#include <wpi/SmallString.h>
+#include <wpi/StringRef.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/ScopedTracer.h"
+#include "gtest/gtest.h"
+
+wpi::SmallString<128> buf;
+wpi::raw_svector_ostream os(buf);
+
+#ifdef __APPLE__
+TEST(ScopedTracerTest, DISABLED_Timing) {
+#else
+TEST(ScopedTracerTest, Timing) {
+#endif
+  {
+    frc::ScopedTracer tracer("timing_test", os);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1500));
+  }
+
+  wpi::StringRef out = os.str();
+  EXPECT_TRUE(out.startswith("	timing_test: 1.5"));
+}

wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp

diff --git a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
index ae253a7..dea56bb 100644
--- a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
+++ b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,13 +7,18 @@
 
 #include <thread>
 
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+
 #include "frc/SlewRateLimiter.h"
+#include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
 
 TEST(SlewRateLimiterTest, SlewRateLimitTest) {
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1.0_s);
 
   EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
 }
@@ -21,7 +26,7 @@
 TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1.0_s);
 
   EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
 }

wpilibc/src/test/native/cpp/TimedRobotTest.cpp

diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
new file mode 100644
index 0000000..f91efb8
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
@@ -0,0 +1,394 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/TimedRobot.h"  // NOLINT(build/include_order)
+
+#include <stdint.h>
+
+#include <atomic>
+#include <thread>
+
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace {
+class TimedRobotTest : public ::testing::Test {
+ protected:
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+class MockRobot : public TimedRobot {
+ public:
+  std::atomic<uint32_t> m_robotInitCount{0};
+  std::atomic<uint32_t> m_simulationInitCount{0};
+  std::atomic<uint32_t> m_disabledInitCount{0};
+  std::atomic<uint32_t> m_autonomousInitCount{0};
+  std::atomic<uint32_t> m_teleopInitCount{0};
+  std::atomic<uint32_t> m_testInitCount{0};
+
+  std::atomic<uint32_t> m_robotPeriodicCount{0};
+  std::atomic<uint32_t> m_simulationPeriodicCount{0};
+  std::atomic<uint32_t> m_disabledPeriodicCount{0};
+  std::atomic<uint32_t> m_autonomousPeriodicCount{0};
+  std::atomic<uint32_t> m_teleopPeriodicCount{0};
+  std::atomic<uint32_t> m_testPeriodicCount{0};
+
+  void RobotInit() override { m_robotInitCount++; }
+
+  void SimulationInit() override { m_simulationInitCount++; }
+
+  void DisabledInit() override { m_disabledInitCount++; }
+
+  void AutonomousInit() override { m_autonomousInitCount++; }
+
+  void TeleopInit() override { m_teleopInitCount++; }
+
+  void TestInit() override { m_testInitCount++; }
+
+  void RobotPeriodic() override { m_robotPeriodicCount++; }
+
+  void SimulationPeriodic() override { m_simulationPeriodicCount++; }
+
+  void DisabledPeriodic() override { m_disabledPeriodicCount++; }
+
+  void AutonomousPeriodic() override { m_autonomousPeriodicCount++; }
+
+  void TeleopPeriodic() override { m_teleopPeriodicCount++; }
+
+  void TestPeriodic() override { m_testPeriodicCount++; }
+};
+}  // namespace
+
+TEST_F(TimedRobotTest, Disabled) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(2u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Autonomous) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetTest(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(2u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Teleop) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(1u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(2u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Test) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(1u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(2u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, AddPeriodic) {
+  MockRobot robot;
+
+  std::atomic<uint32_t> callbackCount{0};
+  robot.AddPeriodic([&] { callbackCount++; }, 10_ms);
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, callbackCount);
+
+  frc::sim::StepTiming(10_ms);
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, callbackCount);
+
+  frc::sim::StepTiming(10_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(2u, callbackCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, AddPeriodicWithOffset) {
+  MockRobot robot;
+
+  std::atomic<uint32_t> callbackCount{0};
+  robot.AddPeriodic([&] { callbackCount++; }, 10_ms, 5_ms);
+
+  // Expirations in this test (ms)
+  //
+  // Robot | Callback
+  // ================
+  //    20 |      15
+  //    40 |      25
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, callbackCount);
+
+  frc::sim::StepTiming(7.5_ms);
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, callbackCount);
+
+  frc::sim::StepTiming(7.5_ms);
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, callbackCount);
+
+  frc::sim::StepTiming(5_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, callbackCount);
+
+  frc::sim::StepTiming(5_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(2u, callbackCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}

wpilibc/src/test/native/cpp/WatchdogTest.cpp

diff --git a/wpilibc/src/test/native/cpp/WatchdogTest.cpp b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
index 10ff996..0e33511 100644
--- a/wpilibc/src/test/native/cpp/WatchdogTest.cpp
+++ b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,101 +9,92 @@
 
 #include <stdint.h>
 
-#include <thread>
-
-#include <wpi/raw_ostream.h>
-
+#include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_EnableDisable) {
-#else
-TEST(WatchdogTest, EnableDisable) {
-#endif
+namespace {
+class WatchdogTest : public ::testing::Test {
+ protected:
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+}  // namespace
+
+TEST_F(WatchdogTest, EnableDisable) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
-  wpi::outs() << "Run 1\n";
+  // Run 1
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
-  wpi::outs() << "Run 2\n";
+  // Run 2
   watchdogCounter = 0;
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(600));
+  frc::sim::StepTiming(0.4_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 
-  wpi::outs() << "Run 3\n";
+  // Run 3
   watchdogCounter = 0;
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_Reset) {
-#else
-TEST(WatchdogTest, Reset) {
-#endif
+TEST_F(WatchdogTest, Reset) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Reset();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_SetTimeout) {
-#else
-TEST(WatchdogTest, SetTimeout) {
-#endif
+TEST_F(WatchdogTest, SetTimeout) {
   uint32_t watchdogCounter = 0;
 
-  Watchdog watchdog(1.0_s, [&] { watchdogCounter++; });
+  Watchdog watchdog(1_s, [&] { watchdogCounter++; });
 
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.SetTimeout(0.2_s);
 
   EXPECT_EQ(0.2, watchdog.GetTimeout());
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.3_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_IsExpired) {
-#else
-TEST(WatchdogTest, IsExpired) {
-#endif
+TEST_F(WatchdogTest, IsExpired) {
   Watchdog watchdog(0.2_s, [] {});
   EXPECT_FALSE(watchdog.IsExpired());
   watchdog.Enable();
 
   EXPECT_FALSE(watchdog.IsExpired());
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.3_s);
   EXPECT_TRUE(watchdog.IsExpired());
 
   watchdog.Disable();
@@ -113,43 +104,35 @@
   EXPECT_FALSE(watchdog.IsExpired());
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_Epochs) {
-#else
-TEST(WatchdogTest, Epochs) {
-#endif
+TEST_F(WatchdogTest, Epochs) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
-  wpi::outs() << "Run 1\n";
+  // Run 1
   watchdog.Enable();
   watchdog.AddEpoch("Epoch 1");
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  frc::sim::StepTiming(0.1_s);
   watchdog.AddEpoch("Epoch 2");
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  frc::sim::StepTiming(0.1_s);
   watchdog.AddEpoch("Epoch 3");
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
-  wpi::outs() << "Run 2\n";
+  // Run 2
   watchdog.Enable();
   watchdog.AddEpoch("Epoch 1");
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Reset();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.AddEpoch("Epoch 2");
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_MultiWatchdog) {
-#else
-TEST(WatchdogTest, MultiWatchdog) {
-#endif
+TEST_F(WatchdogTest, MultiWatchdog) {
   uint32_t watchdogCounter1 = 0;
   uint32_t watchdogCounter2 = 0;
 
@@ -157,13 +140,13 @@
   Watchdog watchdog2(0.6_s, [&] { watchdogCounter2++; });
 
   watchdog2.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.25_s);
   EXPECT_EQ(0u, watchdogCounter1) << "Watchdog triggered early";
   EXPECT_EQ(0u, watchdogCounter2) << "Watchdog triggered early";
 
   // Sleep enough such that only the watchdog enabled later times out first
   watchdog1.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.25_s);
   watchdog1.Disable();
   watchdog2.Disable();
 

wpilibc/src/test/native/cpp/XboxControllerTest.cpp

diff --git a/wpilibc/src/test/native/cpp/XboxControllerTest.cpp b/wpilibc/src/test/native/cpp/XboxControllerTest.cpp
new file mode 100644
index 0000000..66ccfcd
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/XboxControllerTest.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/XboxController.h"  // NOLINT(build/include_order)
+
+#include "frc/simulation/XboxControllerSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(XboxControllerTests, GetX) {
+  XboxController joy{2};
+  sim::XboxControllerSim joysim{joy};
+
+  joysim.SetX(XboxController::kLeftHand, 0.35);
+  joysim.SetX(XboxController::kRightHand, 0.45);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetX(XboxController::kLeftHand), 0.35, 0.001);
+  ASSERT_NEAR(joy.GetX(XboxController::kRightHand), 0.45, 0.001);
+}
+
+TEST(XboxControllerTests, GetBumper) {
+  XboxController joy{1};
+  sim::XboxControllerSim joysim{joy};
+
+  joysim.SetBumper(XboxController::kLeftHand, false);
+  joysim.SetBumper(XboxController::kRightHand, true);
+  joysim.NotifyNewData();
+  ASSERT_FALSE(joy.GetBumper(XboxController::kLeftHand));
+  ASSERT_TRUE(joy.GetBumper(XboxController::kRightHand));
+  // need to call pressed and released to clear flags
+  joy.GetBumperPressed(XboxController::kLeftHand);
+  joy.GetBumperReleased(XboxController::kLeftHand);
+  joy.GetBumperPressed(XboxController::kRightHand);
+  joy.GetBumperReleased(XboxController::kRightHand);
+
+  joysim.SetBumper(XboxController::kLeftHand, true);
+  joysim.SetBumper(XboxController::kRightHand, false);
+  joysim.NotifyNewData();
+  ASSERT_TRUE(joy.GetBumper(XboxController::kLeftHand));
+  ASSERT_TRUE(joy.GetBumperPressed(XboxController::kLeftHand));
+  ASSERT_FALSE(joy.GetBumperReleased(XboxController::kLeftHand));
+  ASSERT_FALSE(joy.GetBumper(XboxController::kRightHand));
+  ASSERT_FALSE(joy.GetBumperPressed(XboxController::kRightHand));
+  ASSERT_TRUE(joy.GetBumperReleased(XboxController::kRightHand));
+}
+
+TEST(XboxControllerTests, GetAButton) {
+  XboxController joy{1};
+  sim::XboxControllerSim joysim{joy};
+
+  joysim.SetAButton(false);
+  joysim.NotifyNewData();
+  ASSERT_FALSE(joy.GetAButton());
+  // need to call pressed and released to clear flags
+  joy.GetAButtonPressed();
+  joy.GetAButtonReleased();
+
+  joysim.SetAButton(true);
+  joysim.NotifyNewData();
+  ASSERT_TRUE(joy.GetAButton());
+  ASSERT_TRUE(joy.GetAButtonPressed());
+  ASSERT_FALSE(joy.GetAButtonReleased());
+
+  joysim.SetAButton(false);
+  joysim.NotifyNewData();
+  ASSERT_FALSE(joy.GetAButton());
+  ASSERT_FALSE(joy.GetAButtonPressed());
+  ASSERT_TRUE(joy.GetAButtonReleased());
+}

wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp

diff --git a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
new file mode 100644
index 0000000..00259d1
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <units/angle.h>
+
+#include "frc/controller/ControllerUtil.h"
+#include "gtest/gtest.h"
+
+TEST(ControllerUtilTest, GetModulusError) {
+  // Test symmetric range
+  EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0, frc::GetModulusError(-170.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
+
+  // Test range starting at zero
+  EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0, 190.0 + 360.0, 0.0, 360.0));
+
+  // Test asymmetric range that doesn't start at zero
+  EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -170.0, 190.0));
+
+  // Test all supported types
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError<double>(170.0, -170.0, -170.0, 190.0));
+  EXPECT_EQ(-20, frc::GetModulusError<int>(170, -170, -170, 190));
+  EXPECT_EQ(-20_deg, frc::GetModulusError<units::degree_t>(170_deg, -170_deg,
+                                                           -170_deg, 190_deg));
+}

wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp

diff --git a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
new file mode 100644
index 0000000..fb72a4a
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <units/math.h>
+#include <units/time.h>
+#include <wpi/math>
+
+#include "frc/controller/HolonomicDriveController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
+
+TEST(HolonomicDriveControllerTest, ReachesReference) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+      frc::ProfiledPIDController<units::radian>{
+          1.0, 0.0, 0.0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              6.28_rad_per_s, 3.14_rad_per_s / 1_s}}};
+
+  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},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.0_mps, 4.0_mps_sq});
+
+  constexpr auto kDt = 0.02_s;
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
+
+    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt});
+  }
+
+  auto& endPose = trajectory.States().back().pose;
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(units::math::NormalizeAngle(robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}

wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp

diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index c68cb37..3580308 100644
--- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,7 +21,7 @@
   controller->SetP(1);
   controller->EnableContinuousInput(-180, 180);
 
-  EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
+  EXPECT_LT(controller->Calculate(-179, 179), 0);
 }
 
 TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {

wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp

diff --git a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
new file mode 100644
index 0000000..6f9c36d
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <units/angle.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <wpi/math>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "gtest/gtest.h"
+
+class ProfiledPIDInputOutputTest : public testing::Test {
+ protected:
+  frc::ProfiledPIDController<units::degrees>* controller;
+
+  void SetUp() override {
+    controller = new frc::ProfiledPIDController<units::degrees>(
+        0, 0, 0, {360_deg_per_s, 180_deg_per_s_sq});
+  }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  controller->Reset(-179_deg);
+  EXPECT_LT(controller->Calculate(-179_deg, 179_deg), 0);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest1) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  static constexpr units::degree_t kSetpoint{-179.0};
+  static constexpr units::degree_t kMeasurement{-179.0};
+  static constexpr units::degree_t kGoal{179.0};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            180_deg);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.4826633343199735};
+  static constexpr units::radian_t kMeasurement{-3.1352207333939606};
+  static constexpr units::radian_t kGoal{-3.534162788601621};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.5176604690006377};
+  static constexpr units::radian_t kMeasurement{3.1191729343822456};
+  static constexpr units::radian_t kGoal{2.709680418117445};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutputTest) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg));
+}
+
+TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutputTest) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(0.025_deg, 0_deg);
+  }
+
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutputTest) {
+  controller->SetD(4);
+
+  controller->Calculate(0_deg, 0_deg);
+
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025_deg, 0_deg));
+}

wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp

diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
index a600054..fb34385 100644
--- a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -1,11 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <wpi/math>
+#include <units/math.h>
 
 #include "frc/controller/RamseteController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
@@ -17,16 +17,6 @@
 static constexpr units::meter_t kTolerance{1 / 12.0};
 static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
 
-units::radian_t boundRadians(units::radian_t value) {
-  while (value > units::radian_t{wpi::math::pi}) {
-    value -= units::radian_t{wpi::math::pi * 2};
-  }
-  while (value <= units::radian_t{-wpi::math::pi}) {
-    value += units::radian_t{wpi::math::pi * 2};
-  }
-  return value;
-}
-
 TEST(RamseteControllerTest, ReachesReference) {
   frc::RamseteController controller{2.0, 0.7};
   frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
@@ -47,11 +37,9 @@
   }
 
   auto& endPose = trajectory.States().back().pose;
-  EXPECT_NEAR_UNITS(endPose.Translation().X(), robotPose.Translation().X(),
-                    kTolerance);
-  EXPECT_NEAR_UNITS(endPose.Translation().Y(), robotPose.Translation().Y(),
-                    kTolerance);
-  EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() -
-                                 robotPose.Rotation().Radians()),
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(units::math::NormalizeAngle(endPose.Rotation().Radians() -
+                                                robotPose.Rotation().Radians()),
                     0_rad, kAngularTolerance);
 }

wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp

diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
deleted file mode 100644
index 8b5f674..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <cmath>
-
-#include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Pose2dTest, TransformBy) {
-  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
-  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
-
-  const auto transformed = initial + transform;
-
-  EXPECT_NEAR(transformed.Translation().X().to<double>(),
-              1 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Translation().Y().to<double>(),
-              2 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
-}
-
-TEST(Pose2dTest, RelativeTo) {
-  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
-  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
-
-  const auto finalRelativeToInitial = final.RelativeTo(initial);
-
-  EXPECT_NEAR(finalRelativeToInitial.Translation().X().to<double>(),
-              5.0 * std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Translation().Y().to<double>(), 0.0,
-              kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
-              kEpsilon);
-}
-
-TEST(Pose2dTest, Equality) {
-  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
-  const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
-  EXPECT_TRUE(a == b);
-}
-
-TEST(Pose2dTest, Inequality) {
-  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
-  const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
-  EXPECT_TRUE(a != b);
-}
-
-TEST(Pose2dTest, Minus) {
-  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
-  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
-
-  const auto transform = final - initial;
-
-  EXPECT_NEAR(transform.Translation().X().to<double>(), 5.0 * std::sqrt(2.0),
-              kEpsilon);
-  EXPECT_NEAR(transform.Translation().Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
-}

wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp

diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
deleted file mode 100644
index ba80787..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <cmath>
-
-#include <wpi/math>
-
-#include "frc/geometry/Rotation2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Rotation2dTest, RadiansToDegrees) {
-  const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
-  const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
-
-  EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
-  EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, DegreesToRadians) {
-  const auto one = Rotation2d(45.0_deg);
-  const auto two = Rotation2d(30.0_deg);
-
-  EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
-  EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, RotateByFromZero) {
-  const Rotation2d zero;
-  auto sum = zero + Rotation2d(90.0_deg);
-
-  EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
-  EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, RotateByNonZero) {
-  auto rot = Rotation2d(90.0_deg);
-  rot += Rotation2d(30.0_deg);
-
-  EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, Minus) {
-  const auto one = Rotation2d(70.0_deg);
-  const auto two = Rotation2d(30.0_deg);
-
-  EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, Equality) {
-  const auto one = Rotation2d(43_deg);
-  const auto two = Rotation2d(43_deg);
-  EXPECT_TRUE(one == two);
-}
-
-TEST(Rotation2dTest, Inequality) {
-  const auto one = Rotation2d(43_deg);
-  const auto two = Rotation2d(43.5_deg);
-  EXPECT_TRUE(one != two);
-}

wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp

diff --git a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
deleted file mode 100644
index 99fced7..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <cmath>
-
-#include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Translation2dTest, Sum) {
-  const Translation2d one{1.0_m, 3.0_m};
-  const Translation2d two{2.0_m, 5.0_m};
-
-  const auto sum = one + two;
-
-  EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
-  EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
-}
-
-TEST(Translation2dTest, Difference) {
-  const Translation2d one{1.0_m, 3.0_m};
-  const Translation2d two{2.0_m, 5.0_m};
-
-  const auto difference = one - two;
-
-  EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
-  EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
-}
-
-TEST(Translation2dTest, RotateBy) {
-  const Translation2d another{3.0_m, 0.0_m};
-  const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
-
-  EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
-}
-
-TEST(Translation2dTest, Multiplication) {
-  const Translation2d original{3.0_m, 5.0_m};
-  const auto mult = original * 3;
-
-  EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
-  EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
-}
-
-TEST(Translation2d, Division) {
-  const Translation2d original{3.0_m, 5.0_m};
-  const auto div = original / 2;
-
-  EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
-  EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
-}
-
-TEST(Translation2dTest, Norm) {
-  const Translation2d one{3.0_m, 5.0_m};
-  EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
-}
-
-TEST(Translation2dTest, Distance) {
-  const Translation2d one{1_m, 1_m};
-  const Translation2d two{6_m, 6_m};
-  EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
-}
-
-TEST(Translation2dTest, UnaryMinus) {
-  const Translation2d original{-4.5_m, 7_m};
-  const auto inverted = -original;
-
-  EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
-  EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
-}
-
-TEST(Translation2dTest, Equality) {
-  const Translation2d one{9_m, 5.5_m};
-  const Translation2d two{9_m, 5.5_m};
-  EXPECT_TRUE(one == two);
-}
-
-TEST(Translation2dTest, Inequality) {
-  const Translation2d one{9_m, 5.5_m};
-  const Translation2d two{9_m, 5.7_m};
-  EXPECT_TRUE(one != two);
-}

wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp

diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
deleted file mode 100644
index faf36d9..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <cmath>
-
-#include <wpi/math>
-
-#include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Twist2dTest, Straight) {
-  const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
-  const auto straightPose = Pose2d().Exp(straight);
-
-  EXPECT_NEAR(straightPose.Translation().X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Translation().Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
-}
-
-TEST(Twist2dTest, QuarterCircle) {
-  const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
-                              units::radian_t(wpi::math::pi / 2.0)};
-  const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
-
-  EXPECT_NEAR(quarterCirclePose.Translation().X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Translation().Y().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
-              kEpsilon);
-}
-
-TEST(Twist2dTest, DiagonalNoDtheta) {
-  const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
-  const auto diagonalPose = Pose2d().Exp(diagonal);
-
-  EXPECT_NEAR(diagonalPose.Translation().X().to<double>(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Translation().Y().to<double>(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
-}
-
-TEST(Twist2dTest, Equality) {
-  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
-  const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
-  EXPECT_TRUE(one == two);
-}
-
-TEST(Twist2dTest, Inequality) {
-  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
-  const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
-  EXPECT_TRUE(one != two);
-}
-
-TEST(Twist2dTest, Pose2dLog) {
-  const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
-  const Pose2d start{};
-
-  const auto twist = start.Log(end);
-
-  EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
-  EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
-}

wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp

diff --git a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
deleted file mode 100644
index 3fb63fb..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "gtest/gtest.h"
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(ChassisSpeeds, FieldRelativeConstruction) {
-  const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
-
-  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
-  EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
-  EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
-}

wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp

diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
deleted file mode 100644
index ad0d3c7..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <units/units.h>
-#include <wpi/math>
-
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const ChassisSpeeds chassisSpeeds;
-  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-  EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const DifferentialDriveWheelSpeeds wheelSpeeds;
-  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
-  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-  EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
-  EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
-  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
-                                    units::radians_per_second_t{wpi::math::pi}};
-  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-  EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
-  EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const DifferentialDriveWheelSpeeds wheelSpeeds{
-      units::meters_per_second_t(+0.381 * wpi::math::pi),
-      units::meters_per_second_t(-0.381 * wpi::math::pi)};
-  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
-}

wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp

diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
deleted file mode 100644
index 8acaf92..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <wpi/math>
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "gtest/gtest.h"
-
-static constexpr double kEpsilon = 1E-9;
-
-using namespace frc;
-
-TEST(DifferentialDriveOdometry, EncoderDistances) {
-  DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
-
-  const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
-                                     units::meter_t(5 * wpi::math::pi));
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
-}

wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp

diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
deleted file mode 100644
index 75f395d..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ /dev/null
@@ -1,230 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <units/units.h>
-#include <wpi/math>
-
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class MecanumDriveKinematicsTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  Translation2d m_br{-12_m, -12_m};
-
-  MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-};
-
-TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
-  ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
-  */
-
-  EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
-                                      3.536_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-  By equation (13.13) of the state-space-guide, the chassis motion from wheel
-  velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
-  [[5][0][0]]
-  */
-
-  EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-  By equation (13.12) of the state-space-guide, the wheel speeds should
-  be as follows:
-  velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
-  */
-
-  EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
-                                      -2.828427_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
-    [[5][0][0]]
-  */
-
-  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
-  */
-
-  EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
-                                      -106.62919_mps, 106.62919_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
-    be [[0][0][2pi]]
-  */
-
-  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
-  ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
-  */
-
-  EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
-                                      -13.435_mps, 16.26_mps};
-
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
-    [[2][3][1]]
-  */
-
-  EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
-  */
-
-  EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
-                                      33.941_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the
-    wheel velocities should be [[12][-12][1]]
-  */
-
-  EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest,
-       OffCenterTranslationRotationInverseKinematics) {
-  ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
-  */
-  EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest,
-       OffCenterTranslationRotationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
-                                      36.06_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the
-    wheel velocities should be [[17][-10][1]]
-  */
-
-  EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
-  MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
-  wheelSpeeds.Normalize(5.5_mps);
-
-  double kFactor = 5.5 / 7.0;
-
-  EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
-  EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
-  EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
-  EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
-}

wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp

diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
deleted file mode 100644
index 5d990bf..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/MecanumDriveOdometry.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class MecanumDriveOdometryTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  Translation2d m_br{-12_m, -12_m};
-
-  MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-  MecanumDriveOdometry odometry{kinematics, 0_rad};
-};
-
-TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
-                                      3.536_mps};
-
-  odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
-  auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
-
-  EXPECT_NEAR(secondPose.Translation().X().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(secondPose.Translation().Y().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
-}
-
-TEST_F(MecanumDriveOdometryTest, TwoIterations) {
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
-
-  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
-}
-
-TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
-                                 39.986_mps};
-  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 12, 0.01);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
-  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
-}
-
-TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
-  odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
-
-  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
-
-  odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
-  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
-}

wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp

diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
deleted file mode 100644
index 8fd67ea..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <units/units.h>
-#include <wpi/math>
-
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 0.1;
-
-class SwerveDriveKinematicsTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  Translation2d m_br{-12_m, -12_m};
-
-  SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
-};
-
-TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
-  ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
-
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
-  SwerveModuleState state{5.0_mps, Rotation2d()};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
-  SwerveModuleState state{5_mps, Rotation2d(90_deg)};
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
-  SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
-  SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
-  SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
-  SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
-  SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
-  SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
-  SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
-  SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest,
-       OffCenterCORRotationAndTranslationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
-  auto [fl, fr, bl, br] =
-      m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
-
-  EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest,
-       OffCenterCORRotationAndTranslationForwardKinematics) {
-  SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
-  SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
-  SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
-  SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
-  SwerveModuleState state1{5.0_mps, Rotation2d()};
-  SwerveModuleState state2{6.0_mps, Rotation2d()};
-  SwerveModuleState state3{4.0_mps, Rotation2d()};
-  SwerveModuleState state4{7.0_mps, Rotation2d()};
-
-  std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
-
-  double kFactor = 5.5 / 7.0;
-
-  EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
-  EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
-  EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
-  EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
-}

wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp

diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
deleted file mode 100644
index d0399dc..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "frc/kinematics/SwerveDriveOdometry.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 0.01;
-
-class SwerveDriveOdometryTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  Translation2d m_br{-12_m, -12_m};
-
-  SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
-  SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
-};
-
-TEST_F(SwerveDriveOdometryTest, TwoIterations) {
-  SwerveModuleState state{5_mps, Rotation2d()};
-
-  m_odometry.ResetPosition(Pose2d(), 0_rad);
-  m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
-                            SwerveModuleState(), SwerveModuleState(),
-                            SwerveModuleState());
-  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
-                                        state, state);
-
-  EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
-}
-
-TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
-  SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
-  SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
-  SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
-  SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
-
-  SwerveModuleState zero{0_mps, Rotation2d()};
-
-  m_odometry.ResetPosition(Pose2d(), 0_rad);
-  m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
-  auto pose =
-      m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
-
-  EXPECT_NEAR(12.0, pose.Translation().X().to<double>(), kEpsilon);
-  EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
-  EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
-}
-
-TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
-  m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
-
-  SwerveModuleState state{5_mps, Rotation2d()};
-
-  m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
-                            SwerveModuleState(), SwerveModuleState(),
-                            SwerveModuleState());
-  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
-                                        state, state);
-
-  EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
-}

wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp
new file mode 100644
index 0000000..d2c7ea8
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/BuiltInAccelerometerSim.h"  // NOLINT(build/include_order)
+
+#include <hal/Accelerometer.h>
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+using namespace frc::sim;
+
+TEST(AcclerometerSimTests, TestActiveCallback) {
+  HAL_Initialize(500, 0);
+
+  BuiltInAccelerometerSim sim;
+
+  sim.ResetData();
+
+  bool wasTriggered = false;
+  bool lastValue = false;
+
+  auto cb = sim.RegisterActiveCallback(
+      [&](wpi::StringRef name, const HAL_Value* value) {
+        wasTriggered = true;
+        lastValue = value->data.v_boolean;
+      },
+      false);
+
+  EXPECT_FALSE(wasTriggered);
+
+  HAL_SetAccelerometerActive(true);
+
+  EXPECT_TRUE(wasTriggered);
+  EXPECT_TRUE(lastValue);
+}

wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
new file mode 100644
index 0000000..f32be7e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <hal/HAL.h>
+#include <units/math.h>
+#include <wpi/math>
+
+#include "frc/AnalogEncoder.h"
+#include "frc/AnalogInput.h"
+#include "frc/simulation/AnalogEncoderSim.h"
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+TEST(AnalogEncoderSimTest, TestBasic) {
+  frc::AnalogInput ai(0);
+  frc::AnalogEncoder encoder{ai};
+  frc::sim::AnalogEncoderSim encoderSim{encoder};
+
+  encoderSim.SetPosition(180_deg);
+  EXPECT_NEAR(encoder.Get().to<double>(), 0.5, 1E-8);
+  EXPECT_NEAR(encoderSim.GetTurns().to<double>(), 0.5, 1E-8);
+  EXPECT_NEAR(encoderSim.GetPosition().Radians().to<double>(), wpi::math::pi,
+              1E-8);
+}

wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
new file mode 100644
index 0000000..f09344e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <units/current.h>
+#include <units/math.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/RamseteController.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/simulation/DifferentialDrivetrainSim.h"
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "gtest/gtest.h"
+
+TEST(DifferentialDriveSim, Convergence) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
+  frc::RamseteController ramsete;
+
+  feedforward.Reset(frc::MakeMatrix<2, 1>(0.0, 0.0));
+
+  // Ground truth.
+  Eigen::Matrix<double, 7, 1> groundTruthX =
+      Eigen::Matrix<double, 7, 1>::Zero();
+
+  frc::TrajectoryConfig config{1_mps, 1_mps_sq};
+  config.AddConstraint(
+      frc::DifferentialDriveKinematicsConstraint(kinematics, 1_mps));
+
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      frc::Pose2d(), {}, frc::Pose2d(2_m, 2_m, 0_rad), config);
+
+  for (double t = 0; t < trajectory.TotalTime().to<double>(); t += 0.02) {
+    auto state = trajectory.Sample(20_ms);
+    auto ramseteOut = ramsete.Calculate(sim.GetPose(), state);
+
+    auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
+    auto voltages = feedforward.Calculate(
+        frc::MakeMatrix<2, 1>(l.to<double>(), r.to<double>()));
+
+    // Sim periodic code.
+    sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0)));
+    sim.Update(20_ms);
+
+    // Update ground truth.
+    groundTruthX = frc::RungeKutta(
+        [&sim](const auto& x, const auto& u) -> Eigen::Matrix<double, 7, 1> {
+          return sim.Dynamics(x, u);
+        },
+        groundTruthX, voltages, 20_ms);
+  }
+
+  EXPECT_NEAR(groundTruthX(0, 0), sim.GetState(0), 1E-6);
+  EXPECT_NEAR(groundTruthX(1, 0), sim.GetState(1), 1E-6);
+  EXPECT_NEAR(groundTruthX(2, 0), sim.GetState(2), 1E-6);
+}
+
+TEST(DifferentialDriveSim, Current) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  sim.SetInputs(-12_V, 12_V);
+  for (int i = 0; i < 10; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+
+  sim.SetInputs(12_V, 12_V);
+  for (int i = 0; i < 20; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+
+  sim.SetInputs(-12_V, 12_V);
+  for (int i = 0; i < 30; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+}
+
+TEST(DifferentialDriveSim, ModelStability) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 2_kg_sq_m, 5.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  sim.SetInputs(2_V, 4_V);
+
+  // 10 seconds should be enough time to verify stability
+  for (int i = 0; i < 500; ++i) {
+    sim.Update(20_ms);
+  }
+
+  EXPECT_LT(units::math::abs(sim.GetPose().Translation().Norm()), 100_m);
+}

wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
new file mode 100644
index 0000000..05b0dc9
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+
+#include <units/time.h>
+
+#include "frc/Encoder.h"
+#include "frc/PWMVictorSPX.h"
+#include "frc/RobotController.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/PIDController.h"
+#include "frc/simulation/ElevatorSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "gtest/gtest.h"
+
+TEST(ElevatorSim, StateSpaceSim) {
+  frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg,
+                            units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 3_m,
+                            {0.01});
+  frc2::PIDController controller(10, 0.0, 0.0);
+
+  frc::PWMVictorSPX motor(0);
+  frc::Encoder encoder(0, 1);
+  frc::sim::EncoderSim encoderSim(encoder);
+
+  for (size_t i = 0; i < 100; ++i) {
+    controller.SetSetpoint(2.0);
+    auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
+    motor.Set(nextVoltage / frc::RobotController::GetInputVoltage());
+
+    auto u = frc::MakeMatrix<1, 1>(motor.Get() *
+                                   frc::RobotController::GetInputVoltage());
+    sim.SetInput(u);
+    sim.Update(20_ms);
+
+    const auto& y = sim.GetOutput();
+    encoderSim.SetDistance(y(0));
+  }
+
+  EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().to<double>(), 0.2);
+}
+
+TEST(ElevatorSim, MinMax) {
+  frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg,
+                            units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m,
+                            {0.01});
+  for (size_t i = 0; i < 100; ++i) {
+    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.Update(20_ms);
+
+    auto height = sim.GetPosition();
+    EXPECT_TRUE(height > -0.05_m);
+  }
+
+  for (size_t i = 0; i < 100; ++i) {
+    sim.SetInput(frc::MakeMatrix<1, 1>(12.0));
+    sim.Update(20_ms);
+
+    auto height = sim.GetPosition();
+    EXPECT_TRUE(height < 1.05_m);
+  }
+}

wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp
new file mode 100644
index 0000000..ea7b809
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <hal/SimDevice.h>
+#include <wpi/StringRef.h>
+
+#include "frc/simulation/SimDeviceSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc::sim;
+
+TEST(SimDeviceSimTests, TestBasic) {
+  hal::SimDevice dev{"test"};
+  hal::SimBoolean devBool = dev.CreateBoolean("bool", false, false);
+
+  SimDeviceSim sim{"test"};
+  hal::SimBoolean simBool = sim.GetBoolean("bool");
+  EXPECT_FALSE(simBool.Get());
+  simBool.Set(true);
+  EXPECT_TRUE(devBool.Get());
+}
+
+TEST(SimDeviceSimTests, TestEnumerateDevices) {
+  hal::SimDevice dev{"test"};
+
+  bool foundit = false;
+  SimDeviceSim::EnumerateDevices(
+      "te", [&](const char* name, HAL_SimDeviceHandle handle) {
+        if (wpi::StringRef(name) == "test") foundit = true;
+      });
+  EXPECT_TRUE(foundit);
+}

wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp b/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
new file mode 100644
index 0000000..1b86e05
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <exception>
+
+#include <hal/HAL.h>
+
+#include "frc/simulation/AddressableLEDSim.h"
+#include "frc/simulation/AnalogGyroSim.h"
+#include "frc/simulation/AnalogInputSim.h"
+#include "frc/simulation/AnalogOutputSim.h"
+#include "frc/simulation/AnalogTriggerSim.h"
+#include "frc/simulation/BuiltInAccelerometerSim.h"
+#include "frc/simulation/DIOSim.h"
+#include "frc/simulation/DigitalPWMSim.h"
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/DutyCycleSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/simulation/PCMSim.h"
+#include "frc/simulation/PDPSim.h"
+#include "frc/simulation/PWMSim.h"
+#include "frc/simulation/RelaySim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "frc/simulation/SPIAccelerometerSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc::sim;
+
+TEST(SimInitializationTests, TestAllInitialize) {
+  HAL_Initialize(500, 0);
+  BuiltInAccelerometerSim biacsim;
+  AnalogGyroSim agsim{0};
+  AnalogInputSim aisim{0};
+  AnalogOutputSim aosim{0};
+  EXPECT_THROW(AnalogTriggerSim::CreateForChannel(0), std::out_of_range);
+  EXPECT_THROW(DigitalPWMSim::CreateForChannel(0), std::out_of_range);
+  DIOSim diosim{0};
+  DriverStationSim dssim;
+  (void)dssim;
+  EncoderSim esim = EncoderSim::CreateForIndex(0);
+  (void)esim;
+  PCMSim pcmsim{0};
+  PDPSim pdpsim{0};
+  PWMSim pwmsim{0};
+  RelaySim rsim{0};
+  RoboRioSim rrsim;
+  (void)rrsim;
+  SPIAccelerometerSim sasim{0};
+  DutyCycleSim dcsim = DutyCycleSim::CreateForIndex(0);
+  (void)dcsim;
+  AddressableLEDSim adLED;
+}

wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
new file mode 100644
index 0000000..a553985
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/simulation/SingleJointedArmSim.h"
+#include "gtest/gtest.h"
+
+TEST(SingleJointedArmTest, Disabled) {
+  frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m,
+                                    9.5_in, -180_deg, 0_deg, 10_lb, true);
+  sim.SetState(frc::MakeMatrix<2, 1>(0.0, 0.0));
+
+  for (size_t i = 0; i < 12 / 0.02; ++i) {
+    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.Update(20_ms);
+  }
+
+  // The arm should swing down.
+  EXPECT_NEAR(sim.GetAngle().to<double>(), -wpi::math::pi / 2, 0.01);
+}

wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp

diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
new file mode 100644
index 0000000..5766751
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+
+#include "frc/Encoder.h"
+#include "frc/PWMVictorSPX.h"
+#include "frc/RobotController.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "frc/simulation/BatterySim.h"
+#include "frc/simulation/DifferentialDrivetrainSim.h"
+#include "frc/simulation/ElevatorSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/simulation/FlywheelSim.h"
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/simulation/PWMSim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "frc/simulation/SingleJointedArmSim.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "gtest/gtest.h"
+
+TEST(StateSpaceSimTest, TestFlywheelSim) {
+  const frc::LinearSystem<1, 1, 1> plant =
+      frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(
+          0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq);
+  frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0};
+  frc2::PIDController controller{0.2, 0.0, 0.0};
+  frc::SimpleMotorFeedforward<units::radian> feedforward{
+      0_V, 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
+  frc::Encoder encoder{0, 1};
+  frc::sim::EncoderSim encoderSim{encoder};
+  frc::PWMVictorSPX motor{0};
+
+  for (int i = 0; i < 100; i++) {
+    // RobotPeriodic runs first
+    auto voltageOut = controller.Calculate(encoder.GetRate(), 200.0);
+    motor.SetVoltage(units::volt_t(voltageOut) +
+                     feedforward.Calculate(200_rad_per_s));
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInput(frc::MakeMatrix<1, 1>(
+        motor.Get() * frc::RobotController::GetInputVoltage()));
+    sim.Update(20_ms);
+    encoderSim.SetRate(sim.GetAngularVelocity().to<double>());
+  }
+
+  ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1);
+}

wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp

diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
deleted file mode 100644
index f2f0a38..0000000
--- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <chrono>
-#include <iostream>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Rotation2d.h"
-#include "frc/spline/QuinticHermiteSpline.h"
-#include "frc/spline/SplineHelper.h"
-#include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-namespace frc {
-class CubicHermiteSplineTest : public ::testing::Test {
- protected:
-  static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
-                  const Pose2d& b) {
-    // Start the timer.
-    const auto start = std::chrono::high_resolution_clock::now();
-
-    // Generate and parameterize the spline.
-
-    const auto [startCV, endCV] =
-        SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b);
-
-    const auto splines =
-        SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
-    std::vector<Spline<3>::PoseWithCurvature> poses;
-
-    poses.push_back(splines[0].GetPoint(0.0));
-
-    for (auto&& spline : splines) {
-      auto x = SplineParameterizer::Parameterize(spline);
-      poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
-    }
-
-    // End timer.
-    const auto finish = std::chrono::high_resolution_clock::now();
-
-    // Calculate the duration (used when benchmarking)
-    const auto duration =
-        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
-
-    for (unsigned int i = 0; i < poses.size() - 1; i++) {
-      auto& p0 = poses[i];
-      auto& p1 = poses[i + 1];
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      auto twist = p0.first.Log(p1.first);
-      EXPECT_LT(std::abs(twist.dx.to<double>()),
-                SplineParameterizer::kMaxDx.to<double>());
-      EXPECT_LT(std::abs(twist.dy.to<double>()),
-                SplineParameterizer::kMaxDy.to<double>());
-      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
-                SplineParameterizer::kMaxDtheta.to<double>());
-    }
-
-    // Check first point.
-    EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
-                a.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
-                a.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
-                a.Rotation().Radians().to<double>(), 1E-9);
-
-    // Check interior waypoints
-    bool interiorsGood = true;
-    for (auto& waypoint : waypoints) {
-      bool found = false;
-      for (auto& state : poses) {
-        if (std::abs(
-                waypoint.Distance(state.first.Translation()).to<double>()) <
-            1E-9) {
-          found = true;
-        }
-      }
-      interiorsGood &= found;
-    }
-
-    EXPECT_TRUE(interiorsGood);
-
-    // Check last point.
-    EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
-                b.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
-                b.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
-
-    static_cast<void>(duration);
-  }
-};
-}  // namespace frc
-
-TEST_F(CubicHermiteSplineTest, StraightLine) {
-  Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
-}
-
-TEST_F(CubicHermiteSplineTest, SCurve) {
-  Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
-  std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
-                                       Translation2d(2_m, -1_m)};
-  Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
-  Run(start, waypoints, end);
-}
-
-TEST_F(CubicHermiteSplineTest, OneInterior) {
-  Pose2d start{0_m, 0_m, 0_rad};
-  std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
-  Pose2d end{4_m, 0_m, 0_rad};
-  Run(start, waypoints, end);
-}
-
-TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
-  EXPECT_THROW(
-      Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
-          Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
-      SplineParameterizer::MalformedSplineException);
-  EXPECT_THROW(
-      Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
-          Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
-      SplineParameterizer::MalformedSplineException);
-}

wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp

diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
deleted file mode 100644
index 8a87053..0000000
--- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <chrono>
-#include <iostream>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Rotation2d.h"
-#include "frc/spline/QuinticHermiteSpline.h"
-#include "frc/spline/SplineHelper.h"
-#include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-namespace frc {
-class QuinticHermiteSplineTest : public ::testing::Test {
- protected:
-  static void Run(const Pose2d& a, const Pose2d& b) {
-    // Start the timer.
-    const auto start = std::chrono::high_resolution_clock::now();
-
-    // Generate and parameterize the spline.
-    const auto spline = SplineHelper::QuinticSplinesFromControlVectors(
-        SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0];
-    const auto poses = SplineParameterizer::Parameterize(spline);
-
-    // End timer.
-    const auto finish = std::chrono::high_resolution_clock::now();
-
-    // Calculate the duration (used when benchmarking)
-    const auto duration =
-        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
-
-    for (unsigned int i = 0; i < poses.size() - 1; i++) {
-      auto& p0 = poses[i];
-      auto& p1 = poses[i + 1];
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      auto twist = p0.first.Log(p1.first);
-      EXPECT_LT(std::abs(twist.dx.to<double>()),
-                SplineParameterizer::kMaxDx.to<double>());
-      EXPECT_LT(std::abs(twist.dy.to<double>()),
-                SplineParameterizer::kMaxDy.to<double>());
-      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
-                SplineParameterizer::kMaxDtheta.to<double>());
-    }
-
-    // Check first point.
-    EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
-                a.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
-                a.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
-                a.Rotation().Radians().to<double>(), 1E-9);
-
-    // Check last point.
-    EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
-                b.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
-                b.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
-
-    static_cast<void>(duration);
-  }
-};
-}  // namespace frc
-
-TEST_F(QuinticHermiteSplineTest, StraightLine) {
-  Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
-}
-
-TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
-  Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
-}
-
-TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
-  Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
-      Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
-}
-
-TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
-  EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
-                   Pose2d(1_m, 0_m, Rotation2d(180_deg))),
-               SplineParameterizer::MalformedSplineException);
-  EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
-                   Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
-               SplineParameterizer::MalformedSplineException);
-}

wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp

diff --git a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
deleted file mode 100644
index 37f471a..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <memory>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(CentripetalAccelerationConstraintTest, Constraint) {
-  const auto maxCentripetalAcceleration = 7_fps_sq;
-
-  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
-  config.AddConstraint(
-      CentripetalAccelerationConstraint(maxCentripetalAcceleration));
-
-  auto trajectory = TestTrajectory::GetTrajectory(config);
-
-  units::second_t time = 0_s;
-  units::second_t dt = 20_ms;
-  units::second_t duration = trajectory.TotalTime();
-
-  while (time < duration) {
-    const Trajectory::State point = trajectory.Sample(time);
-    time += dt;
-
-    auto centripetalAcceleration =
-        units::math::pow<2>(point.velocity) * point.curvature / 1_rad;
-
-    EXPECT_TRUE(centripetalAcceleration <
-                maxCentripetalAcceleration + 0.05_mps_sq);
-  }
-}

wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp

diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
deleted file mode 100644
index df5ddbd..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <memory>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
-  const auto maxVelocity = 12_fps;
-  const DifferentialDriveKinematics kinematics{27_in};
-
-  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
-  config.AddConstraint(
-      DifferentialDriveKinematicsConstraint(kinematics, maxVelocity));
-
-  auto trajectory = TestTrajectory::GetTrajectory(config);
-
-  units::second_t time = 0_s;
-  units::second_t dt = 20_ms;
-  units::second_t duration = trajectory.TotalTime();
-
-  while (time < duration) {
-    const Trajectory::State point = trajectory.Sample(time);
-    time += dt;
-
-    const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
-                                      point.velocity * point.curvature};
-
-    auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-    EXPECT_TRUE(left < maxVelocity + 0.05_mps);
-    EXPECT_TRUE(right < maxVelocity + 0.05_mps);
-  }
-}

wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp

diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
deleted file mode 100644
index eb230da..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <iostream>
-#include <memory>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
-  // Pick an unreasonably large kA to ensure the constraint has to do some work
-  SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
-                                                   3_V / 1_mps_sq};
-  const DifferentialDriveKinematics kinematics{0.5_m};
-  const auto maxVoltage = 10_V;
-
-  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
-  config.AddConstraint(
-      DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
-
-  auto trajectory = TestTrajectory::GetTrajectory(config);
-
-  units::second_t time = 0_s;
-  units::second_t dt = 20_ms;
-  units::second_t duration = trajectory.TotalTime();
-
-  while (time < duration) {
-    const Trajectory::State point = trajectory.Sample(time);
-    time += dt;
-
-    const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
-                                      point.velocity * point.curvature};
-
-    auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-    // Not really a strictly-correct test as we're using the chassis accel
-    // instead of the wheel accel, but much easier than doing it "properly" and
-    // a reasonable check anyway
-    EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
-                maxVoltage + 0.05_V);
-    EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
-                -maxVoltage - 0.05_V);
-    EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
-                maxVoltage + 0.05_V);
-    EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
-                -maxVoltage - 0.05_V);
-  }
-}

wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp

diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
deleted file mode 100644
index 90046c5..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(TrajectoryGenerationTest, ObeysConstraints) {
-  TrajectoryConfig config{12_fps, 12_fps_sq};
-  auto trajectory = TestTrajectory::GetTrajectory(config);
-
-  units::second_t time = 0_s;
-  units::second_t dt = 20_ms;
-  units::second_t duration = trajectory.TotalTime();
-
-  while (time < duration) {
-    const Trajectory::State point = trajectory.Sample(time);
-    time += dt;
-
-    EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps);
-    EXPECT_TRUE(units::math::abs(point.acceleration) <=
-                12_fps_sq + 0.01_fps_sq);
-  }
-}
-
-TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
-  const auto t = TrajectoryGenerator::GenerateTrajectory(
-      std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
-                          Pose2d(1_m, 0_m, Rotation2d(180_deg))},
-      TrajectoryConfig(12_fps, 12_fps_sq));
-
-  ASSERT_EQ(t.States().size(), 1u);
-  ASSERT_EQ(t.TotalTime(), 0_s);
-}

wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp

diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
deleted file mode 100644
index 999d2bb..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/TrajectoryConfig.h"
-#include "frc/trajectory/TrajectoryUtil.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(TrajectoryJsonTest, DeserializeMatches) {
-  TrajectoryConfig config{12_fps, 12_fps_sq};
-  auto trajectory = TestTrajectory::GetTrajectory(config);
-
-  Trajectory deserialized;
-  EXPECT_NO_THROW(deserialized = TrajectoryUtil::DeserializeTrajectory(
-                      TrajectoryUtil::SerializeTrajectory(trajectory)));
-  EXPECT_EQ(trajectory.States(), deserialized.States());
-}

wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp

diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
deleted file mode 100644
index af69b58..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryConfig.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
-
-void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
-                              std::vector<frc::Trajectory::State> statesB) {
-  for (unsigned int i = 0; i < statesA.size() - 1; i++) {
-    auto a1 = statesA[i].pose;
-    auto a2 = statesA[i + 1].pose;
-
-    auto b1 = statesB[i].pose;
-    auto b2 = statesB[i + 1].pose;
-
-    auto a = a2.RelativeTo(a1);
-    auto b = b2.RelativeTo(b1);
-
-    EXPECT_NEAR(a.Translation().X().to<double>(),
-                b.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(a.Translation().Y().to<double>(),
-                b.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(a.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
-  }
-}
-
-TEST(TrajectoryTransforms, TransformBy) {
-  frc::TrajectoryConfig config{3_mps, 3_mps_sq};
-  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
-      config);
-
-  auto transformedTrajectory =
-      trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
-
-  auto firstPose = transformedTrajectory.Sample(0_s).pose;
-
-  EXPECT_NEAR(firstPose.Translation().X().to<double>(), 1.0, 1E-9);
-  EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 2.0, 1E-9);
-  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
-
-  TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
-}
-
-TEST(TrajectoryTransforms, RelativeTo) {
-  frc::TrajectoryConfig config{3_mps, 3_mps_sq};
-  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
-      frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
-
-  auto transformedTrajectory =
-      trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
-
-  auto firstPose = transformedTrajectory.Sample(0_s).pose;
-
-  EXPECT_NEAR(firstPose.Translation().X().to<double>(), 0, 1E-9);
-  EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 0, 1E-9);
-  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
-
-  TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
-}

wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp

diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
deleted file mode 100644
index bd9ffef..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ /dev/null
@@ -1,233 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/trajectory/TrapezoidProfile.h"  // NOLINT(build/include_order)
-
-#include <chrono>
-#include <cmath>
-
-#include "gtest/gtest.h"
-
-static constexpr auto kDt = 10_ms;
-
-#define EXPECT_NEAR_UNITS(val1, val2, eps) \
-  EXPECT_LE(units::math::abs(val1 - val2), eps)
-
-#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
-  if (val1 <= val2) {                            \
-    EXPECT_LE(val1, val2);                       \
-  } else {                                       \
-    EXPECT_NEAR_UNITS(val1, val2, eps);          \
-  }
-
-TEST(TrapezoidProfileTest, ReachesGoal) {
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
-  frc::TrapezoidProfile<units::meter>::State state;
-
-  for (int i = 0; i < 450; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-// Tests that decreasing the maximum velocity in the middle when it is already
-// moving faster than the new max is handled correctly
-TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
-
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
-
-  auto lastPos = state.position;
-  for (int i = 0; i < 1600; ++i) {
-    if (i == 400) {
-      constraints.maxVelocity = 0.75_mps;
-    }
-
-    profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
-    state = profile.Calculate(kDt);
-    auto estimatedVel = (state.position - lastPos) / kDt;
-
-    if (i >= 400) {
-      // Since estimatedVel can have floating point rounding errors, we check
-      // whether value is less than or within an error delta of the new
-      // constraint.
-      EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
-
-      EXPECT_LE(state.velocity, constraints.maxVelocity);
-    }
-
-    lastPos = state.position;
-  }
-  EXPECT_EQ(state, goal);
-}
-
-// There is some somewhat tricky code for dealing with going backwards
-TEST(TrapezoidProfileTest, Backwards) {
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
-  frc::TrapezoidProfile<units::meter>::State state;
-
-  for (int i = 0; i < 400; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
-  frc::TrapezoidProfile<units::meter>::State state;
-
-  for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_NE(state, goal);
-
-  goal = {0.0_m, 0.0_mps};
-  for (int i = 0; i < 550; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-// Checks to make sure that it hits top speed
-TEST(TrapezoidProfileTest, TopSpeed) {
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
-  frc::TrapezoidProfile<units::meter>::State state;
-
-  for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
-
-  for (int i = 0; i < 2000; ++i) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-TEST(TrapezoidProfileTest, TimingToCurrent) {
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
-  frc::TrapezoidProfile<units::meter>::State state;
-
-  for (int i = 0; i < 400; i++) {
-    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-    EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingToGoal) {
-  using units::unit_cast;
-
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
-
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
-
-  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
-  bool reachedGoal = false;
-  for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
-    if (!reachedGoal && state == goal) {
-      // Expected value using for loop index is just an approximation since the
-      // time left in the profile doesn't increase linearly at the endpoints
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
-      reachedGoal = true;
-    }
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingBeforeGoal) {
-  using units::unit_cast;
-
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
-
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
-
-  auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
-  bool reachedGoal = false;
-  for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
-    if (!reachedGoal &&
-        (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
-      reachedGoal = true;
-    }
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
-  using units::unit_cast;
-
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
-
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
-
-  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
-  bool reachedGoal = false;
-  for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
-    if (!reachedGoal && state == goal) {
-      // Expected value using for loop index is just an approximation since the
-      // time left in the profile doesn't increase linearly at the endpoints
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
-      reachedGoal = true;
-    }
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
-  using units::unit_cast;
-
-  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
-
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
-
-  auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
-  bool reachedGoal = false;
-  for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
-    if (!reachedGoal &&
-        (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
-      reachedGoal = true;
-    }
-  }
-}

wpilibc/src/test/native/include/trajectory/TestTrajectory.h

diff --git a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
deleted file mode 100644
index 4a496d7..0000000
--- a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-class TestTrajectory {
- public:
-  static Trajectory GetTrajectory(TrajectoryConfig& config) {
-    // 2018 cross scale auto waypoints
-    const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
-    const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
-
-    config.SetReversed(true);
-
-    auto vector = std::vector<Translation2d>{
-        (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
-            .Translation(),
-        (sideStart +
-         Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
-            .Translation()};
-
-    return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,
-                                                   crossScale, config);
-  }
-};
-
-}  // namespace frc

wpilibcExamples/build.gradle

diff --git a/wpilibcExamples/build.gradle b/wpilibcExamples/build.gradle
index c6c1dff..35f20d4 100644
--- a/wpilibcExamples/build.gradle
+++ b/wpilibcExamples/build.gradle
@@ -57,6 +57,7 @@
                 lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                 lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                 lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                 lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                 lib project: ':cscore', library: 'cscore', linkage: 'shared'
                 project(':hal').addHalDependency(binary, 'shared')
@@ -84,16 +85,12 @@
                     lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                     lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                     lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                     lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                     lib project: ':cscore', library: 'cscore', linkage: 'shared'
                     project(':hal').addHalDependency(binary, 'shared')
                     lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                    if (binary.targetPlatform.name != nativeUtils.wpi.platforms.roborio) {
-                        lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
-                        lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
-                        lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
-                    }
                     if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                         nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                     }
@@ -131,6 +128,7 @@
                     lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                     lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                     lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                     lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                     lib project: ':cscore', library: 'cscore', linkage: 'shared'
                     project(':hal').addHalDependency(binary, 'shared')
@@ -143,11 +141,6 @@
                             cppCompiler.args "/wd4996"
                         }
                     }
-                    if (binary.targetPlatform.name != nativeUtils.wpi.platforms.roborio) {
-                        lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
-                        lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
-                        lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
-                    }
                     if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                         nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
                     }
@@ -177,41 +170,19 @@
                 binary.tasks.install.doLast {
                     if (binary.targetPlatform.operatingSystem.isWindows()) {
                         // Windows batch scripts
-                        def fileName = binary.component.name + 'LowFi.bat'
-                        def file = new File(installDir + fileName)
-                        file.withWriter { out ->
-                            out.println '@ECHO OFF'
-                            out.print 'SET HALSIM_EXTENSIONS='
-                            out.print '"' + new File(installDir + 'lib\\halsim_lowfi.dll').toString() + '";'
-                            out.println '"' + new File(installDir + 'lib\\halsim_ds_nt.dll').toString() + '"'
-                            out.println runFile + ' %*'
-                        }
-
-                        fileName = binary.component.name + 'LowFiRealDS.bat'
+                        fileName = binary.component.name + 'RealDS.bat'
                         file = new File(installDir + fileName)
                         file.withWriter { out ->
                             out.println '@ECHO OFF'
                             out.print 'SET HALSIM_EXTENSIONS='
-                            out.print '"' + new File(installDir + 'lib\\halsim_lowfi.dll').toString() + '";'
                             out.println '"' + new File(installDir + 'lib\\halsim_ds_socket.dll').toString() + '"'
                             out.println runFile + ' %*'
                         }
                     } else {
-                        def fileName = binary.component.name + 'LowFi.sh'
-                        def file = new File(installDir + fileName)
-
-                        file.withWriter { out ->
-                            out.print 'export HALSIM_EXTENSIONS='
-                            out.print '"' + new File(installDir + '/lib/libhalsim_lowfi.so').toString() + '";'
-                            out.println '"' + new File(installDir + '/lib/libhalsim_ds_nt.so').toString() + '"'
-                            out.println runFile + ' "$@"'
-                        }
-
-                        fileName = binary.component.name + 'LowFiRealDS.sh'
+                        fileName = binary.component.name + 'RealDS.sh'
                         file = new File(installDir + fileName)
                         file.withWriter { out ->
                             out.print 'export HALSIM_EXTENSIONS='
-                            out.print '"' + new File(installDir + '/lib/libhalsim_lowfi.so').toString() + '":'
                             out.println '"' + new File(installDir + '/lib/libhalsim_ds_socket.so').toString() + '"'
                             out.println runFile + ' "$@"'
                         }

wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp b/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
index bf54a0a..5e21e56 100644
--- a/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMeInstantCommand2::ReplaceMeInstantCommand2() {
   // Use addRequirements() here to declare subsystem dependencies.
 }

wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp b/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
index aa99a3c..3361a2c 100644
--- a/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMeParallelCommandGroup::ReplaceMeParallelCommandGroup() {
   // Add your commands here, e.g.
   // AddCommands(FooCommand(), BarCommand());

wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp b/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
index 1cfcfa0..5c3466c 100644
--- a/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMeParallelDeadlineGroup::ReplaceMeParallelDeadlineGroup()
     : CommandHelper(
           // The deadline command

wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp b/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
index f3f1d86..6d977f9 100644
--- a/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMeParallelRaceGroup::ReplaceMeParallelRaceGroup() {
   // Add your commands here, e.g.
   // AddCommands(FooCommand(), BarCommand());

wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp b/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
index b465b1e..b39be7a 100644
--- a/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,17 +9,18 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMePIDCommand::ReplaceMePIDCommand()
-    : CommandHelper(frc2::PIDController(0, 0, 0),
-                    // This should return the measurement
-                    [] { return 0; },
-                    // This should return the setpoint (can also be a constant)
-                    [] { return 0; },
-                    // This uses the output
-                    [](double output) {
-                      // Use the output here
-                    }) {}
+    : CommandHelper(
+          frc2::PIDController(0, 0, 0),
+          // This should return the measurement
+          [] { return 0; },
+          // This should return the setpoint (can also be a constant)
+          [] { return 0; },
+          // This uses the output
+          [](double output) {
+            // Use the output here
+          }) {}
 
 // Returns true when the command should end.
 bool ReplaceMePIDCommand::IsFinished() { return false; }

wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp b/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
index 392e643..eada0ac 100644
--- a/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,13 +7,16 @@
 
 #include "ReplaceMeProfiledPIDCommand.h"
 
+#include <units/acceleration.h>
+#include <units/velocity.h>
+
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMeProfiledPIDCommand::ReplaceMeProfiledPIDCommand()
     : CommandHelper(
           // The ProfiledPIDController that the command will use
-          frc::ProfiledPIDController<units::meter>(
+          frc::ProfiledPIDController<units::meters>(
               // The PID gains
               0, 0, 0,
               // The motion profile constraints
@@ -22,11 +25,11 @@
           [] { return 0_m; },
           // This should return the goal state (can also be a constant)
           [] {
-            return frc::TrapezoidProfile<units::meter>::State{0_m, 0_mps};
+            return frc::TrapezoidProfile<units::meters>::State{0_m, 0_mps};
           },
           // This uses the output and current trajectory setpoint
           [](double output,
-             frc::TrapezoidProfile<units::meter>::State setpoint) {
+             frc::TrapezoidProfile<units::meters>::State setpoint) {
             // Use the output and setpoint here
           }) {}
 

wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h

diff --git a/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h b/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h
index d3407e8..dc2c50e 100644
--- a/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h
+++ b/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,9 +9,10 @@
 
 #include <frc2/command/CommandHelper.h>
 #include <frc2/command/ProfiledPIDCommand.h>
+#include <units/length.h>
 
 class ReplaceMeProfiledPIDCommand
-    : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::meter>,
+    : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::meters>,
                                  ReplaceMeProfiledPIDCommand> {
  public:
   ReplaceMeProfiledPIDCommand();

wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp b/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
index dc27b43..4ba8baf 100644
--- a/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,17 +7,20 @@
 
 #include "ReplaceMeProfiledPIDSubsystem.h"
 
+#include <units/acceleration.h>
+#include <units/velocity.h>
+
 ReplaceMeProfiledPIDSubsystem::ReplaceMeProfiledPIDSubsystem()
     : ProfiledPIDSubsystem(
           // The ProfiledPIDController used by the subsystem
-          frc::ProfiledPIDController<units::meter>(
+          frc::ProfiledPIDController<units::meters>(
               // The PID gains
               0, 0, 0,
               // The constraints for the motion profiles
               {0_mps, 0_mps_sq})) {}
 
 void ReplaceMeProfiledPIDSubsystem::UseOutput(
-    double output, frc::TrapezoidProfile<units::meter>::State setpoint) {
+    double output, frc::TrapezoidProfile<units::meters>::State setpoint) {
   // Use the output and current trajectory setpoint here
 }
 

wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h b/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
index b382b1d..6969c29 100644
--- a/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,15 +8,16 @@
 #pragma once
 
 #include <frc2/command/ProfiledPIDSubsystem.h>
+#include <units/length.h>
 
 class ReplaceMeProfiledPIDSubsystem
-    : public frc2::ProfiledPIDSubsystem<units::meter> {
+    : public frc2::ProfiledPIDSubsystem<units::meters> {
  public:
   ReplaceMeProfiledPIDSubsystem();
 
  protected:
   void UseOutput(double output,
-                 frc::TrapezoidProfile<units::meter>::State setpoint) override;
+                 frc::TrapezoidProfile<units::meters>::State setpoint) override;
 
   units::meter_t GetMeasurement() override;
 };

wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp b/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
index 3a5fadc..c7d0702 100644
--- a/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMeSequentialCommandGroup::ReplaceMeSequentialCommandGroup() {
   // Add your commands here, e.g.
   // AddCommands(FooCommand(), BarCommand());

wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
index b8cb678..a337013 100644
--- a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,19 +7,22 @@
 
 #include "ReplaceMeTrapezoidProfileCommand.h"
 
+#include <units/acceleration.h>
+#include <units/velocity.h>
+
 // NOTE:  Consider using this command inline, rather than writing a subclass.
 // For more information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 ReplaceMeTrapezoidProfileCommand::ReplaceMeTrapezoidProfileCommand()
     : CommandHelper
       // The profile to execute
-      (frc::TrapezoidProfile<units::meter>(
+      (frc::TrapezoidProfile<units::meters>(
            // The maximum velocity and acceleration of the profile
            {5_mps, 5_mps_sq},
            // The goal state of the profile
            {10_m, 0_mps},
            // The initial state of the profile
            {0_m, 0_mps}),
-       [](frc::TrapezoidProfile<units::meter>::State state) {
+       [](frc::TrapezoidProfile<units::meters>::State state) {
          // Use the computed intermediate trajectory state here
        }) {}

wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h

diff --git a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
index e9043be..5cf7a20 100644
--- a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
+++ b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,9 +9,10 @@
 
 #include <frc2/command/CommandHelper.h>
 #include <frc2/command/TrapezoidProfileCommand.h>
+#include <units/length.h>
 
 class ReplaceMeTrapezoidProfileCommand
-    : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meter>,
+    : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
                                  ReplaceMeTrapezoidProfileCommand> {
  public:
   ReplaceMeTrapezoidProfileCommand();

wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp
index 162c02f..8bc1bee 100644
--- a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,9 @@
 
 #include "ReplaceMeTrapezoidProfileSubsystem.h"
 
+#include <units/acceleration.h>
+#include <units/velocity.h>
+
 ReplaceMeTrapezoidProfileSubsystem::ReplaceMeTrapezoidProfileSubsystem()
     : TrapezoidProfileSubsystem(
           // The motion profile constraints

wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
index e93f590..42a8cb5 100644
--- a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,7 @@
 #pragma once
 
 #include <frc2/command/TrapezoidProfileSubsystem.h>
+#include <units/length.h>
 
 class ReplaceMeTrapezoidProfileSubsystem
     : frc2::TrapezoidProfileSubsystem<units::meters> {

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

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
index cd19aeb..be32fbb 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -27,7 +27,7 @@
  * can use it to reset any subsystem information you want to clear when the
  * robot is disabled.
  */
-void Robot::DisabledInit() {}
+void Robot::DisabledInit() { m_container.DisablePIDSubsystems(); }
 
 void Robot::DisabledPeriodic() {}
 

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

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
index aec6b1d..d3a0133 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <frc/shuffleboard/Shuffleboard.h>
 #include <frc2/command/button/JoystickButton.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 RobotContainer::RobotContainer() {
   // Initialize all of your commands and subsystems here
@@ -32,12 +32,25 @@
 
   // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
   frc2::JoystickButton(&m_driverController, 1)
-      .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
+      .WhenPressed(
+          [this] {
+            m_arm.SetGoal(2_rad);
+            m_arm.Enable();
+          },
+          {&m_arm});
 
   // Move the arm to neutral position when the 'B' button is pressed.
-  frc2::JoystickButton(&m_driverController, 1)
-      .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
-                   {&m_arm});
+  frc2::JoystickButton(&m_driverController, 2)
+      .WhenPressed(
+          [this] {
+            m_arm.SetGoal(ArmConstants::kArmOffset);
+            m_arm.Enable();
+          },
+          {&m_arm});
+
+  // Disable the arm controller when Y is pressed.
+  frc2::JoystickButton(&m_driverController, 4)
+      .WhenPressed([this] { m_arm.Disable(); }, {&m_arm});
 
   // While holding the shoulder button, drive at half speed
   frc2::JoystickButton(&m_driverController, 6)
@@ -45,6 +58,8 @@
       .WhenReleased([this] { m_drive.SetMaxOutput(1); });
 }
 
+void RobotContainer::DisablePIDSubsystems() { m_arm.Disable(); }
+
 frc2::Command* RobotContainer::GetAutonomousCommand() {
   // Runs the chosen command in autonomous
   return new frc2::InstantCommand([] {});

wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
index 674633c..ac5ffe0 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,10 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/time.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 /**

wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
index fa07359..c238cb6 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -35,6 +35,12 @@
  public:
   RobotContainer();
 
+  /**
+   * Disables all ProfiledPIDSubsystem and PIDSubsystem instances.
+   * This should be called on robot disable to prevent integral windup.
+   */
+  void DisablePIDSubsystems();
+
   frc2::Command* GetAutonomousCommand();
 
  private:

wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
index 693f0b4..1733efe 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 #include <frc/PWMVictorSPX.h>
 #include <frc/controller/ArmFeedforward.h>
 #include <frc2/command/ProfiledPIDSubsystem.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 /**
  * A robot arm subsystem that moves with a motion profile.

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

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
index aec6b1d..ba7bbf5 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <frc/shuffleboard/Shuffleboard.h>
 #include <frc2/command/button/JoystickButton.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 RobotContainer::RobotContainer() {
   // Initialize all of your commands and subsystems here

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
index 674633c..ac5ffe0 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,10 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/time.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 /**

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
index 7c8d261..f0e888d 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -48,9 +48,9 @@
   /**
    * Places this motor controller in follower mode.
    *
-   * @param master The master to follow.
+   * @param leader The leader to follow.
    */
-  void Follow(ExampleSmartMotorController master) {}
+  void Follow(ExampleSmartMotorController leader) {}
 
   /**
    * Returns the encoder distance.

wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
index 52f15f1..8030cb4 100644
--- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <frc/controller/ArmFeedforward.h>
 #include <frc2/command/TrapezoidProfileSubsystem.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 #include "ExampleSmartMotorController.h"
 

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

diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
new file mode 100644
index 0000000..18df257
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/Encoder.h>
+#include <frc/GenericHID.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/RobotController.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/PIDController.h>
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/RoboRioSim.h>
+#include <frc/simulation/SingleJointedArmSim.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/moment_of_inertia.h>
+#include <wpi/math>
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control an arm.
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr int kMotorPort = 0;
+  static constexpr int kEncoderAChannel = 0;
+  static constexpr int kEncoderBChannel = 1;
+  static constexpr int kJoystickPort = 0;
+
+  // The P gain for the PID controller that drives this arm.
+  static constexpr double kArmKp = 5.0;
+
+  // distance per pulse = (angle per revolution) / (pulses per revolution)
+  //  = (2 * PI rads) / (4096 pulses)
+  static constexpr double kArmEncoderDistPerPulse =
+      2.0 * wpi::math::pi / 4096.0;
+
+  // The arm gearbox represents a gerbox containing two Vex 775pro motors.
+  frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2);
+
+  // Standard classes for controlling our arm
+  frc2::PIDController m_controller{kArmKp, 0, 0};
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::Joystick m_joystick{kJoystickPort};
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  // This sim represents an arm with 2 775s, a 100:1 reduction, a mass of 5kg,
+  // 30in overall arm length, range of motion nin [-180, 0] degrees, and noise
+  // with a standard deviation of 0.5 degrees.
+  frc::sim::SingleJointedArmSim m_armSim{
+      m_armGearbox,
+      100.0,
+      frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 5_kg),
+      30_in,
+      -180_deg,
+      0_deg,
+      5_kg,
+      true,
+      {(0.5_deg).to<double>()}};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+
+ public:
+  void RobotInit() { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); }
+
+  void SimulationPeriodic() {
+    // In this method, we update our simulation of what our arm is doing
+    // First, we set our "inputs" (voltages)
+    m_armSim.SetInput(frc::MakeMatrix<1, 1>(
+        m_motor.Get() * frc::RobotController::GetInputVoltage()));
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_armSim.Update(20_ms);
+
+    // Finally, we set our simulated encoder's readings and simulated battery
+    // voltage
+    m_encoderSim.SetDistance(m_armSim.GetAngle().to<double>());
+    // SimBattery estimates loaded battery voltages
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
+  }
+
+  void TeleopPeriodic() {
+    if (m_joystick.GetTrigger()) {
+      // Here, we run PID control like normal, with a constant setpoint of 30in.
+      double pidOutput =
+          m_controller.Calculate(m_encoder.GetDistance(), (30_in).to<double>());
+      m_motor.SetVoltage(units::volt_t(pidOutput));
+    } else {
+      // Otherwise, we disable the motor.
+      m_motor.Set(0.0);
+    }
+  }
+
+  void DisabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_motor.Set(0.0);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

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

diff --git a/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
index a4030a5..fb3999b 100644
--- a/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -54,7 +54,7 @@
     int32_t remaining = 0;
     int32_t status = 0;
     // Update our sample. remaining is the number of samples remaining in the
-    // buffer status is more specfic error messages if readStatus is not OK.
+    // buffer status is more specific error messages if readStatus is not OK.
     // Wait 1ms if buffer is empty
     HAL_DMAReadStatus readStatus =
         sample.Update(&m_dma, 1_ms, &remaining, &status);

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

diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
index 4d9c1ed..c639539 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,13 +8,15 @@
 #include "Drivetrain.h"
 
 void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
-  const auto leftOutput = m_leftPIDController.Calculate(
+  const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  const double leftOutput = m_leftPIDController.Calculate(
       m_leftEncoder.GetRate(), speeds.left.to<double>());
-  const auto rightOutput = m_rightPIDController.Calculate(
+  const double rightOutput = m_rightPIDController.Calculate(
       m_rightEncoder.GetRate(), speeds.right.to<double>());
 
-  m_leftGroup.Set(leftOutput);
-  m_rightGroup.Set(rightOutput);
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
 }
 
 void Drivetrain::Drive(units::meters_per_second_t xSpeed,
@@ -23,6 +25,7 @@
 }
 
 void Drivetrain::UpdateOdometry() {
-  m_odometry.Update(GetAngle(), units::meter_t(m_leftEncoder.GetDistance()),
+  m_odometry.Update(m_gyro.GetRotation2d(),
+                    units::meter_t(m_leftEncoder.GetDistance()),
                     units::meter_t(m_rightEncoder.GetDistance()));
 }

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

diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
index b1e858e..22d1cf6 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
@@ -1,10 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/SlewRateLimiter.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 
@@ -20,14 +21,16 @@
   void TeleopPeriodic() override {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    const auto xSpeed =
-        -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+    const auto xSpeed = -m_speedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_drive.Drive(xSpeed, rot);
@@ -35,6 +38,12 @@
 
  private:
   frc::XboxController m_controller{0};
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
   Drivetrain m_drive;
 };
 

wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h

diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
index 3032bd9..d8f3521 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,9 +12,13 @@
 #include <frc/PWMVictorSPX.h>
 #include <frc/SpeedControllerGroup.h>
 #include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
 #include <wpi/math>
 
 /**
@@ -36,14 +40,6 @@
     m_rightEncoder.Reset();
   }
 
-  /**
-   * Get the robot angle as a Rotation2d.
-   */
-  frc::Rotation2d GetAngle() const {
-    // Negating the angle because WPILib Gyros are CW positive.
-    return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
-  }
-
   static constexpr units::meters_per_second_t kMaxSpeed =
       3.0_mps;  // 3 meters per second
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
@@ -59,13 +55,13 @@
   static constexpr double kWheelRadius = 0.0508;  // meters
   static constexpr int kEncoderResolution = 4096;
 
-  frc::PWMVictorSPX m_leftMaster{1};
+  frc::PWMVictorSPX m_leftLeader{1};
   frc::PWMVictorSPX m_leftFollower{2};
-  frc::PWMVictorSPX m_rightMaster{3};
+  frc::PWMVictorSPX m_rightLeader{3};
   frc::PWMVictorSPX m_rightFollower{4};
 
-  frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower};
-  frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower};
+  frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
 
   frc::Encoder m_leftEncoder{0, 1};
   frc::Encoder m_rightEncoder{2, 3};
@@ -76,5 +72,9 @@
   frc::AnalogGyro m_gyro{0};
 
   frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
-  frc::DifferentialDriveOdometry m_odometry{GetAngle()};
+  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
 };

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
index cd8b3e4..d0bdef6 100644
--- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,16 +10,16 @@
 using namespace DriveConstants;
 
 DriveSubsystem::DriveSubsystem()
-    : m_leftMaster{kLeftMotor1Port},
-      m_leftSlave{kLeftMotor2Port},
-      m_rightMaster{kRightMotor1Port},
-      m_rightSlave{kRightMotor2Port},
+    : m_leftLeader{kLeftMotor1Port},
+      m_leftFollower{kLeftMotor2Port},
+      m_rightLeader{kRightMotor1Port},
+      m_rightFollower{kRightMotor2Port},
       m_feedforward{ks, kv, ka} {
-  m_leftSlave.Follow(m_leftMaster);
-  m_rightSlave.Follow(m_rightMaster);
+  m_leftFollower.Follow(m_leftLeader);
+  m_rightFollower.Follow(m_rightLeader);
 
-  m_leftMaster.SetPID(kp, 0, 0);
-  m_rightMaster.SetPID(kp, 0, 0);
+  m_leftLeader.SetPID(kp, 0, 0);
+  m_rightLeader.SetPID(kp, 0, 0);
 }
 
 void DriveSubsystem::Periodic() {
@@ -29,10 +29,10 @@
 void DriveSubsystem::SetDriveStates(
     frc::TrapezoidProfile<units::meters>::State left,
     frc::TrapezoidProfile<units::meters>::State right) {
-  m_leftMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+  m_leftLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
                            left.position.to<double>(),
                            m_feedforward.Calculate(left.velocity) / 12_V);
-  m_rightMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+  m_rightLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
                             right.position.to<double>(),
                             m_feedforward.Calculate(right.velocity) / 12_V);
 }
@@ -42,16 +42,16 @@
 }
 
 void DriveSubsystem::ResetEncoders() {
-  m_leftMaster.ResetEncoder();
-  m_rightMaster.ResetEncoder();
+  m_leftLeader.ResetEncoder();
+  m_rightLeader.ResetEncoder();
 }
 
 units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
-  return units::meter_t(m_leftMaster.GetEncoderDistance());
+  return units::meter_t(m_leftLeader.GetEncoderDistance());
 }
 
 units::meter_t DriveSubsystem::GetRightEncoderDistance() {
-  return units::meter_t(m_rightMaster.GetEncoderDistance());
+  return units::meter_t(m_rightLeader.GetEncoderDistance());
 }
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
index bcf3356..8ff887e 100644
--- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,11 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 /**

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h

diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
index 7c8d261..f0e888d 100644
--- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -48,9 +48,9 @@
   /**
    * Places this motor controller in follower mode.
    *
-   * @param master The master to follow.
+   * @param leader The leader to follow.
    */
-  void Follow(ExampleSmartMotorController master) {}
+  void Follow(ExampleSmartMotorController leader) {}
 
   /**
    * Returns the encoder distance.

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
index f68cbbd..45fa982 100644
--- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/trajectory/TrapezoidProfile.h>
 #include <frc2/command/SubsystemBase.h>
-#include <units/units.h>
+#include <units/length.h>
 
 #include "Constants.h"
 #include "ExampleSmartMotorController.h"
@@ -77,14 +77,14 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  ExampleSmartMotorController m_leftMaster;
-  ExampleSmartMotorController m_leftSlave;
-  ExampleSmartMotorController m_rightMaster;
-  ExampleSmartMotorController m_rightSlave;
+  ExampleSmartMotorController m_leftLeader;
+  ExampleSmartMotorController m_leftFollower;
+  ExampleSmartMotorController m_rightLeader;
+  ExampleSmartMotorController m_rightFollower;
 
   // A feedforward component for the drive
   frc::SimpleMotorFeedforward<units::meters> m_feedforward;
 
   // The robot's drive
-  frc::DifferentialDrive m_drive{m_leftMaster, m_rightMaster};
+  frc::DifferentialDrive m_drive{m_leftLeader, m_rightLeader};
 };

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

diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
index d423dd8..b1bb1ec 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,10 @@
 #include <frc/TimedRobot.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <units/units.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
 #include <wpi/math>
 
 class Robot : public frc::TimedRobot {

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

diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
new file mode 100644
index 0000000..1f67449
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/Encoder.h>
+#include <frc/GenericHID.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/RobotController.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/PIDController.h>
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/ElevatorSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/RoboRioSim.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/moment_of_inertia.h>
+#include <wpi/math>
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control an arm.
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr int kMotorPort = 0;
+  static constexpr int kEncoderAChannel = 0;
+  static constexpr int kEncoderBChannel = 1;
+  static constexpr int kJoystickPort = 0;
+
+  static constexpr double kElevatorKp = 5.0;
+  static constexpr double kElevatorGearing = 10.0;
+  static constexpr units::meter_t kElevatorDrumRadius = 2_in;
+  static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
+
+  static constexpr units::meter_t kMinElevatorHeight = 0_in;
+  static constexpr units::meter_t kMaxElevatorHeight = 50_in;
+
+  // distance per pulse = (distance per revolution) / (pulses per revolution)
+  //  = (Pi * D) / ppr
+  static constexpr double kArmEncoderDistPerPulse =
+      2.0 * wpi::math::pi * kElevatorDrumRadius.to<double>() / 4096.0;
+
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
+
+  // Standard classes for controlling our elevator
+  frc2::PIDController m_controller{kElevatorKp, 0, 0};
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::Joystick m_joystick{kJoystickPort};
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
+                                      kElevatorGearing,
+                                      kCarriageMass,
+                                      kElevatorDrumRadius,
+                                      kMinElevatorHeight,
+                                      kMaxElevatorHeight,
+                                      {0.01}};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+
+ public:
+  void RobotInit() { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); }
+
+  void SimulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_elevatorSim.SetInput(frc::MakeMatrix<1, 1>(
+        m_motor.Get() * frc::RobotController::GetInputVoltage()));
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_elevatorSim.Update(20_ms);
+
+    // Finally, we set our simulated encoder's readings and simulated battery
+    // voltage
+    m_encoderSim.SetDistance(m_elevatorSim.GetPosition().to<double>());
+    // SimBattery estimates loaded battery voltages
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
+  }
+
+  void TeleopPeriodic() {
+    if (m_joystick.GetTrigger()) {
+      // Here, we run PID control like normal, with a constant setpoint of 30in.
+      double pidOutput =
+          m_controller.Calculate(m_encoder.GetDistance(), (30_in).to<double>());
+      m_motor.SetVoltage(units::volt_t(pidOutput));
+    } else {
+      // Otherwise, we disable the motor.
+      m_motor.Set(0.0);
+    }
+  }
+
+  void DisabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_motor.Set(0.0);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

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

diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
index 5853054..5addac4 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,11 @@
 #include <frc/TimedRobot.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <units/units.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 #include "ExampleSmartMotorController.h"
@@ -33,8 +37,8 @@
     // Create a motion profile with the given maximum velocity and maximum
     // acceleration constraints for the next setpoint, the desired goal, and the
     // current setpoint.
-    frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
-                                                m_setpoint};
+    frc::TrapezoidProfile<units::meters> profile{m_constraints, m_goal,
+                                                 m_setpoint};
 
     // Retrieve the profiled setpoint for the next timestep. This setpoint moves
     // toward the goal while obeying the constraints.

wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h

diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
index 7c8d261..f0e888d 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -48,9 +48,9 @@
   /**
    * Places this motor controller in follower mode.
    *
-   * @param master The master to follow.
+   * @param leader The leader to follow.
    */
-  void Follow(ExampleSmartMotorController master) {}
+  void Follow(ExampleSmartMotorController leader) {}
 
   /**
    * Returns the encoder distance.

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
index 1aef3ad..0e2d0e7 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,9 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/time.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 /**

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
index e8edd28..7553544 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 #include <frc/PWMVictorSPX.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc2/command/PIDSubsystem.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 class ShooterSubsystem : public frc2::PIDSubsystem {
  public:

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

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

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
index 7f2a60f..e4ebc14 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 #include "subsystems/Claw.h"
 
+#include <frc/smartdashboard/SmartDashboard.h>
+
 Claw::Claw() {
   // Let's show everything on the LiveWindow
   SetName("Claw");
@@ -21,4 +23,8 @@
 
 bool Claw::IsGripping() { return m_contact.Get(); }
 
-void Claw::Log() {}
+void Claw::Log() {
+  frc::SmartDashboard::PutBoolean("Claw switch", IsGripping());
+}
+
+void Claw::Periodic() { Log(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
index 8eec077..6bc946c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -67,3 +67,5 @@
   // Really meters in simulation since it's a rangefinder...
   return m_rangefinder.GetAverageVoltage();
 }
+
+void DriveTrain::Periodic() { Log(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
index acdd07c..d5e4ddd 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -31,3 +31,5 @@
 void Elevator::UseOutput(double output, double setpoint) {
   m_motor.Set(output);
 }
+
+void Elevator::Periodic() { Log(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
index eeb94bb..a335a63 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,9 +23,11 @@
 }
 
 void Wrist::Log() {
-  // frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
+  frc::SmartDashboard::PutNumber("Wrist Angle", GetMeasurement());
 }
 
 double Wrist::GetMeasurement() { return m_pot.Get(); }
 
 void Wrist::UseOutput(double output, double setpoint) { m_motor.Set(output); }
+
+void Wrist::Periodic() { Log(); }

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
index 063ca43..a660c1c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -41,8 +41,17 @@
    */
   bool IsGripping();
 
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
   void Log();
 
+  /**
+   * Log the data periodically. This method is automatically called
+   * by the subsystem.
+   */
+  void Periodic() override;
+
  private:
   frc::PWMVictorSPX m_motor{7};
   frc::DigitalInput m_contact{5};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
index a80429d..0fed552 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -60,6 +60,12 @@
    */
   double GetDistanceToObstacle();
 
+  /**
+   * Log the data periodically. This method is automatically called
+   * by the subsystem.
+   */
+  void Periodic() override;
+
  private:
   frc::PWMVictorSPX m_frontLeft{1};
   frc::PWMVictorSPX m_rearLeft{2};

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
index 8acecbb..5d2cbd1 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -39,6 +39,12 @@
    */
   void UseOutput(double output, double setpoint) override;
 
+  /**
+   * Log the data periodically. This method is automatically called
+   * by the subsystem.
+   */
+  void Periodic() override;
+
  private:
   frc::PWMVictorSPX m_motor{5};
   double m_setpoint = 0;

wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h

diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
index 8b54e7e..795d2a4 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,11 +32,16 @@
 
   /**
    * Use the motor as the PID output. This method is automatically called
-   * by
-   * the subsystem.
+   * by the subsystem.
    */
   void UseOutput(double output, double setpoint) override;
 
+  /**
+   * Log the data periodically. This method is automatically called
+   * by the subsystem.
+   */
+  void Periodic() override;
+
  private:
   frc::PWMVictorSPX m_motor{6};
   double m_setpoint = 0;

wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
index 988535c..0be1f73 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,15 +12,16 @@
 using namespace DriveConstants;
 
 TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
-    : CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD),
-                    // Close loop on heading
-                    [drive] { return drive->GetHeading().to<double>(); },
-                    // Set reference to target
-                    target.to<double>(),
-                    // Pipe output to turn robot
-                    [drive](double output) { drive->ArcadeDrive(0, output); },
-                    // Require the drive
-                    {drive}) {
+    : CommandHelper(
+          frc2::PIDController(kTurnP, kTurnI, kTurnD),
+          // Close loop on heading
+          [drive] { return drive->GetHeading().to<double>(); },
+          // Set reference to target
+          target.to<double>(),
+          // Pipe output to turn robot
+          [drive](double output) { drive->ArcadeDrive(0, output); },
+          // Require the drive
+          {drive}) {
   // Set the controller to be continuous (because it is an angle controller)
   m_controller.EnableContinuousInput(-180, 180);
   // Set the controller tolerance - the delta tolerance ensures the robot is

wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
index f423640..cc8b841 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,8 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
 #include <wpi/math>
 
 /**

wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
index c4cd21c..cd3d667 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 #include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc2/command/SubsystemBase.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 #include "Constants.h"
 

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

diff --git a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
index 738ff32..86b4f24 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 
 /**
  * This is a sample program that uses mecanum drive with a gyro sensor to
- * maintian rotation vectorsin relation to the starting orientation of the robot
+ * maintain rotation vectorsin relation to the starting orientation of the robot
  * (field-oriented controls).
  */
 class Robot : public frc::TimedRobot {

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

diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
index f8719c4..90865ac 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,7 +14,7 @@
   // Initialize all of your commands and subsystems here
 
   // Add commands to the autonomous command chooser
-  m_chooser.AddOption("Simple Auto", &m_simpleAuto);
+  m_chooser.SetDefaultOption("Simple Auto", &m_simpleAuto);
   m_chooser.AddOption("Complex Auto", &m_complexAuto);
 
   // Put the chooser on the dashboard

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

diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
index 9eb9178..d4199ef 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,7 +19,7 @@
   // Initialize all of your commands and subsystems here
 
   // Add commands to the autonomous command chooser
-  m_chooser.AddOption("Simple Auto", &m_simpleAuto);
+  m_chooser.SetDefaultOption("Simple Auto", &m_simpleAuto);
   m_chooser.AddOption("Complex Auto", &m_complexAuto);
 
   // Put the chooser on the dashboard

wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
index 5e4fc8c..b65b1d7 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,19 +15,32 @@
 }
 
 void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
-  const auto frontLeftOutput = m_frontLeftPIDController.Calculate(
+  const auto frontLeftFeedforward =
+      m_feedforward.Calculate(wheelSpeeds.frontLeft);
+  const auto frontRightFeedforward =
+      m_feedforward.Calculate(wheelSpeeds.frontRight);
+  const auto backLeftFeedforward =
+      m_feedforward.Calculate(wheelSpeeds.rearLeft);
+  const auto backRightFeedforward =
+      m_feedforward.Calculate(wheelSpeeds.rearRight);
+
+  const double frontLeftOutput = m_frontLeftPIDController.Calculate(
       m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to<double>());
-  const auto frontRightOutput = m_frontRightPIDController.Calculate(
+  const double frontRightOutput = m_frontRightPIDController.Calculate(
       m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to<double>());
-  const auto backLeftOutput = m_backLeftPIDController.Calculate(
+  const double backLeftOutput = m_backLeftPIDController.Calculate(
       m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to<double>());
-  const auto backRightOutput = m_backRightPIDController.Calculate(
+  const double backRightOutput = m_backRightPIDController.Calculate(
       m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to<double>());
 
-  m_frontLeftMotor.Set(frontLeftOutput);
-  m_frontRightMotor.Set(frontRightOutput);
-  m_backLeftMotor.Set(backLeftOutput);
-  m_backRightMotor.Set(backRightOutput);
+  m_frontLeftMotor.SetVoltage(units::volt_t{frontLeftOutput} +
+                              frontLeftFeedforward);
+  m_frontRightMotor.SetVoltage(units::volt_t{frontRightOutput} +
+                               frontRightFeedforward);
+  m_backLeftMotor.SetVoltage(units::volt_t{backLeftOutput} +
+                             backLeftFeedforward);
+  m_backRightMotor.SetVoltage(units::volt_t{backRightOutput} +
+                              backRightFeedforward);
 }
 
 void Drivetrain::Drive(units::meters_per_second_t xSpeed,
@@ -35,12 +48,12 @@
                        units::radians_per_second_t rot, bool fieldRelative) {
   auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
       fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                          xSpeed, ySpeed, rot, GetAngle())
+                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
   wheelSpeeds.Normalize(kMaxSpeed);
   SetSpeeds(wheelSpeeds);
 }
 
 void Drivetrain::UpdateOdometry() {
-  m_odometry.Update(GetAngle(), GetCurrentState());
+  m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentState());
 }

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

diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
index 5ff14d1..a9695c4 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
@@ -1,10 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/SlewRateLimiter.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 
@@ -23,23 +24,32 @@
   frc::XboxController m_controller{0};
   Drivetrain m_mecanum;
 
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
   void DriveWithJoystick(bool fieldRelative) {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    const auto xSpeed =
-        -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+    const auto xSpeed = -m_xspeedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
-    const auto ySpeed =
-        -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+    const auto ySpeed = -m_yspeedLimiter.Calculate(
+                            m_controller.GetX(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);

wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h

diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
index 6550085..9c5d9e4 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,7 @@
 #include <frc/Encoder.h>
 #include <frc/PWMVictorSPX.h>
 #include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/MecanumDriveKinematics.h>
 #include <frc/kinematics/MecanumDriveOdometry.h>
@@ -24,14 +25,6 @@
  public:
   Drivetrain() { m_gyro.Reset(); }
 
-  /**
-   * Get the robot angle as a Rotation2d
-   */
-  frc::Rotation2d GetAngle() const {
-    // Negating the angle because WPILib Gyros are CW positive.
-    return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
-  }
-
   frc::MecanumDriveWheelSpeeds GetCurrentState() const;
   void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
   void Drive(units::meters_per_second_t xSpeed,
@@ -71,5 +64,9 @@
       m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
       m_backRightLocation};
 
-  frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()};
+  frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d()};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
 };

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

diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
index 756deaa..7746099 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -103,6 +103,9 @@
 
       {&m_drive});
 
+  // Reset odometry to the starting pose of the trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(mecanumControllerCommand),

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
index b758b07..ef6564f 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,8 +7,9 @@
 
 #include "subsystems/DriveSubsystem.h"
 
-#include <frc/geometry/Rotation2d.h>
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 
 #include "Constants.h"
 
@@ -30,9 +31,7 @@
       m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
                          kRearRightEncoderReversed},
 
-      m_odometry{kDriveKinematics,
-                 frc::Rotation2d(units::degree_t(GetHeading())),
-                 frc::Pose2d()} {
+      m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d()} {
   // Set the distance per pulse for the encoders
   m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -43,7 +42,7 @@
 void DriveSubsystem::Periodic() {
   // Implementation of subsystem periodic method goes here.
   m_odometry.Update(
-      frc::Rotation2d(units::degree_t(GetHeading())),
+      m_gyro.GetRotation2d(),
       frc::MecanumDriveWheelSpeeds{
           units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
           units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
@@ -103,19 +102,16 @@
   m_drive.SetMaxOutput(maxOutput);
 }
 
-double DriveSubsystem::GetHeading() {
-  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+units::degree_t DriveSubsystem::GetHeading() const {
+  return m_gyro.GetRotation2d().Degrees();
 }
 
 void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
 
-double DriveSubsystem::GetTurnRate() {
-  return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
-}
+double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
 
 frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
-  m_odometry.ResetPosition(pose,
-                           frc::Rotation2d(units::degree_t(GetHeading())));
+  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
 }

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
index 1baa3de..c37cb8a 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,13 @@
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/MecanumDriveKinematics.h>
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <units/units.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 #pragma once
@@ -50,8 +56,6 @@
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
 
-constexpr bool kGyroReversed = false;
-
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
 // theoretically for *your* robot's drive. The RobotPy Characterization

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
index fb3c19d..1cd72d2 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -103,9 +103,9 @@
   /**
    * Returns the heading of the robot.
    *
-   * @return the robot's heading in degrees, from 180 to 180
+   * @return the robot's heading in degrees, from -180 to 180
    */
-  double GetHeading();
+  units::degree_t GetHeading() const;
 
   /**
    * Zeroes the heading of the robot.

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

diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
index 33d53c6..2f39216 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -84,6 +84,9 @@
       [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
       {&m_drive});
 
+  // Reset odometry to the starting pose of the trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(ramseteCommand),

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
index be5f82a..a82d22d 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,7 +19,7 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
-      m_odometry{frc::Rotation2d(units::degree_t(GetHeading()))} {
+      m_odometry{m_gyro.GetRotation2d()} {
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -29,7 +29,7 @@
 
 void DriveSubsystem::Periodic() {
   // Implementation of subsystem periodic method goes here.
-  m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
+  m_odometry.Update(m_gyro.GetRotation2d(),
                     units::meter_t(m_leftEncoder.GetDistance()),
                     units::meter_t(m_rightEncoder.GetDistance()));
 }
@@ -41,6 +41,7 @@
 void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
   m_leftMotors.SetVoltage(left);
   m_rightMotors.SetVoltage(-right);
+  m_drive.Feed();
 }
 
 void DriveSubsystem::ResetEncoders() {
@@ -60,13 +61,11 @@
   m_drive.SetMaxOutput(maxOutput);
 }
 
-double DriveSubsystem::GetHeading() {
-  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
+units::degree_t DriveSubsystem::GetHeading() const {
+  return m_gyro.GetRotation2d().Degrees();
 }
 
-double DriveSubsystem::GetTurnRate() {
-  return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
-}
+double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
 
 frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
 
@@ -77,6 +76,5 @@
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
   ResetEncoders();
-  m_odometry.ResetPosition(pose,
-                           frc::Rotation2d(units::degree_t(GetHeading())));
+  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
 }

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
index 649082b..7d3ea8f 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,11 @@
 
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
-#include <units/units.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 #pragma once
@@ -41,8 +45,6 @@
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
 
-constexpr bool kGyroReversed = true;
-
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
 // theoretically for *your* robot's drive. The Robot Characterization

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
index d8d1abd..73c6c21 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,7 +15,7 @@
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
 #include <frc2/command/SubsystemBase.h>
-#include <units/units.h>
+#include <units/voltage.h>
 
 #include "Constants.h"
 
@@ -83,9 +83,9 @@
   /**
    * Returns the heading of the robot.
    *
-   * @return the robot's heading in degrees, from 180 to 180
+   * @return the robot's heading in degrees, from -180 to 180
    */
-  double GetHeading();
+  units::degree_t GetHeading() const;
 
   /**
    * Returns the turn rate of the robot.

wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..f52ddec
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drivetrain.h"
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+  const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  const double leftOutput = m_leftPIDController.Calculate(
+      m_leftEncoder.GetRate(), speeds.left.to<double>());
+  const double rightOutput = m_rightPIDController.Calculate(
+      m_rightEncoder.GetRate(), speeds.right.to<double>());
+
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::radians_per_second_t rot) {
+  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_odometry.Update(m_gyro.GetRotation2d(),
+                    units::meter_t(m_leftEncoder.GetDistance()),
+                    units::meter_t(m_rightEncoder.GetDistance()));
+}
+
+void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
+  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+}
+
+frc::Pose2d Drivetrain::GetPose() const { return m_odometry.GetPose(); }

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

diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp
new file mode 100644
index 0000000..61e5d85
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/SlewRateLimiter.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc2/Timer.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousInit() override {
+    // Start the timer.
+    m_timer.Start();
+
+    // Reset the drivetrain's odometry to the starting pose of the trajectory.
+    m_drive.ResetOdometry(m_trajectory.InitialPose());
+  }
+
+  void AutonomousPeriodic() override {
+    // Update odometry.
+    m_drive.UpdateOdometry();
+
+    if (m_timer.Get() < m_trajectory.TotalTime()) {
+      // Get the desired pose from the trajectory.
+      auto desiredPose = m_trajectory.Sample(m_timer.Get());
+
+      // Get the reference chassis speeds from the Ramsete Controller.
+      auto refChassisSpeeds =
+          m_ramseteController.Calculate(m_drive.GetPose(), desiredPose);
+
+      // Set the linear and angular speeds.
+      m_drive.Drive(refChassisSpeeds.vx, refChassisSpeeds.omega);
+    } else {
+      m_drive.Drive(0_mps, 0_rad_per_s);
+    }
+  }
+
+  void TeleopPeriodic() override {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_speedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+                     Drivetrain::kMaxAngularSpeed;
+
+    m_drive.Drive(xSpeed, rot);
+  }
+
+ private:
+  frc::XboxController m_controller{0};
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  Drivetrain m_drive;
+
+  // An example trajectory to follow.
+  frc::Trajectory m_trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      frc::Pose2d(0_m, 0_m, 0_rad),
+      {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+      frc::Pose2d(3_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));
+
+  // The Ramsete Controller to follow the trajectory.
+  frc::RamseteController m_ramseteController;
+
+  // The timer to use during the autonomous period.
+  frc2::Timer m_timer;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h

diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
new file mode 100644
index 0000000..a707014
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <wpi/math>
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() {
+    m_gyro.Reset();
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+                                      kEncoderResolution);
+    m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+                                       kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
+  }
+
+  static constexpr units::meters_per_second_t kMaxSpeed =
+      3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::math::pi};  // 1/2 rotation per second
+
+  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::radians_per_second_t rot);
+  void UpdateOdometry();
+  void ResetOdometry(const frc::Pose2d& pose);
+  frc::Pose2d GetPose() const;
+
+ private:
+  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+  static constexpr double kWheelRadius = 0.0508;  // meters
+  static constexpr int kEncoderResolution = 4096;
+
+  frc::PWMVictorSPX m_leftLeader{1};
+  frc::PWMVictorSPX m_leftFollower{2};
+  frc::PWMVictorSPX m_rightLeader{3};
+  frc::PWMVictorSPX m_rightFollower{4};
+
+  frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+
+  frc::Encoder m_leftEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
+
+  frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+};

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

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
new file mode 100644
index 0000000..5efe04c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/Encoder.h>
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/LinearPlantInversionFeedforward.h>
+#include <frc/controller/LinearQuadraticRegulator.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/estimator/KalmanFilter.h>
+#include <frc/system/LinearSystemLoop.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/angle.h>
+#include <units/moment_of_inertia.h>
+#include <wpi/math>
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control an arm.
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr int kMotorPort = 0;
+  static constexpr int kEncoderAChannel = 0;
+  static constexpr int kEncoderBChannel = 1;
+  static constexpr int kJoystickPort = 0;
+
+  static constexpr units::radian_t kRaisedPosition = 90_deg;
+  static constexpr units::radian_t kLoweredPosition = 0_deg;
+
+  // Moment of inertia of the arm. Can be estimated with CAD. If finding this
+  // constant is difficult, LinearSystem.identifyPositionSystem may be better.
+  static constexpr units::kilogram_square_meter_t kArmMOI = 1.2_kg_sq_m;
+
+  // Reduction between motors and encoder, as output over input. If the arm
+  // spins slower than the motors, this number should be greater than one.
+  static constexpr double kArmGearing = 10.0;
+
+  // The plant holds a state-space model of our arm. This system has the
+  // following properties:
+  //
+  // States: [position, velocity], in radians and radians per second.
+  // Inputs (what we can "put in"): [voltage], in volts.
+  // Outputs (what we can measure): [position], in radians.
+  frc::LinearSystem<2, 1, 1> m_armPlant =
+      frc::LinearSystemId::SingleJointedArmSystem(frc::DCMotor::NEO(2), kArmMOI,
+                                                  kArmGearing);
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  frc::KalmanFilter<2, 1, 1> m_observer{
+      m_armPlant,
+      {0.015, 0.17},  // How accurate we think our model is
+      {0.01},         // How accurate we think our encoder position
+      // data is. In this case we very highly trust our encoder position
+      // reading.
+      20_ms};
+
+  // A LQR uses feedback to create voltage commands.
+  frc::LinearQuadraticRegulator<2, 1> m_controller{
+      m_armPlant,
+      // qelms. Velocity error tolerance, in radians and radians per second.
+      // Decrease this to more heavily penalize state excursion, or make the
+      // controller behave more aggressively.
+      {1.0 * 2.0 * wpi::math::pi / 360.0, 10.0 * 2.0 * wpi::math::pi / 360.0},
+      // relms. Control effort (voltage) tolerance. Decrease this to more
+      // heavily penalize control effort, or make the controller less
+      // aggressive. 12 is a good starting point because that is the
+      // (approximate) maximum voltage of a battery.
+      {12.0},
+      // Nominal time between loops. 20ms for TimedRobot, but can be lower if
+      // using notifiers.
+      20_ms};
+
+  // The state-space loop combines a controller, observer, feedforward and plant
+  // for easy control.
+  frc::LinearSystemLoop<2, 1, 1> m_loop{m_armPlant, m_controller, m_observer,
+                                        12_V, 20_ms};
+
+  // An encoder set up to measure arm position in radians per second.
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+
+  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::XboxController m_joystick{kJoystickPort};
+
+  frc::TrapezoidProfile<units::radians>::Constraints m_constraints{
+      45_deg_per_s, 90_deg_per_s / 1_s};
+
+  frc::TrapezoidProfile<units::radians>::State m_lastProfiledReference;
+
+ public:
+  void RobotInit() {
+    // We go 2 pi radians per 4096 clicks.
+    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0);
+  }
+
+  void TeleopInit() {
+    m_loop.Reset(
+        frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate()));
+
+    m_lastProfiledReference = {
+        units::radian_t(m_encoder.GetDistance()),
+        units::radians_per_second_t(m_encoder.GetRate())};
+  }
+
+  void TeleopPeriodic() {
+    // Sets the target position of our arm. This is similar to setting the
+    // setpoint of a PID controller.
+    frc::TrapezoidProfile<units::radians>::State goal;
+    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+      // We pressed the bumper, so let's set our next reference
+      goal = {kRaisedPosition, 0_rad_per_s};
+    } else {
+      // We released the bumper, so let's spin down
+      goal = {kLoweredPosition, 0_rad_per_s};
+    }
+    m_lastProfiledReference =
+        (frc::TrapezoidProfile<units::radians>(m_constraints, goal,
+                                               m_lastProfiledReference))
+            .Calculate(20_ms);
+
+    m_loop.SetNextR(
+        frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to<double>(),
+                              m_lastProfiledReference.velocity.to<double>()));
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to
+    // predict the next state with out Kalman filter.
+    m_loop.Predict(20_ms);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    m_motor.SetVoltage(units::volt_t(m_loop.U(0)));
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp
new file mode 100644
index 0000000..75849f1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics(
+    kTrackwidth);
+
+const frc::LinearSystem<2, 2, 2> DriveConstants::kDrivetrainPlant =
+    frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular, kaAngular);

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

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
new file mode 100644
index 0000000..26d7499
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Robot.h"
+
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/RoboRioSim.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {
+  frc2::CommandScheduler::GetInstance().CancelAll();
+}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+void Robot::SimulationPeriodic() {
+  // Here we calculate the battery voltage based on drawn current.
+  // As our robot draws more power from the battery its voltage drops.
+  // The estimated voltage is highly dependent on the battery's internal
+  // resistance.
+  auto current = m_container.GetRobotDrive().GetCurrentDraw();
+  auto loadedVoltage = frc::sim::BatterySim::Calculate({current});
+  frc::sim::RoboRioSim::SetVInVoltage(loadedVoltage);
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

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

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..7608122
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/RamseteCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "Constants.h"
+
+RobotContainer::RobotContainer() {
+  // Initialize all of your commands and subsystems here
+
+  // Configure the button bindings
+  ConfigureButtonBindings();
+
+  // Set up default drive command
+  m_drive.SetDefaultCommand(frc2::RunCommand(
+      [this] {
+        m_drive.ArcadeDrive(
+            m_driverController.GetY(frc::GenericHID::kLeftHand),
+            m_driverController.GetX(frc::GenericHID::kRightHand));
+      },
+      {&m_drive}));
+}
+
+const DriveSubsystem& RobotContainer::GetRobotDrive() const { return m_drive; }
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Configure your button bindings here
+
+  // While holding the shoulder button, drive at half speed
+  frc2::JoystickButton(&m_driverController, 6)
+      .WhenPressed(&m_driveHalfSpeed)
+      .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Create a voltage constraint to ensure we don't accelerate too fast
+  frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
+      frc::SimpleMotorFeedforward<units::meters>(
+          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
+      DriveConstants::kDriveKinematics, 10_V);
+
+  // Set up config for trajectory
+  frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+                               AutoConstants::kMaxAcceleration);
+  // Add kinematics to ensure max speed is actually obeyed
+  config.SetKinematics(DriveConstants::kDriveKinematics);
+  // Apply the voltage constraint
+  config.AddConstraint(autoVoltageConstraint);
+
+  // An example trajectory to follow.  All units in meters.
+  auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      // Start at the origin facing the +X direction
+      frc::Pose2d(),
+      // Pass through these two interior waypoints, making an 's' curve path
+      {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+      // End 3 meters straight ahead of where we started, facing forward
+      frc::Pose2d(3_m, 0_m, 0_deg),
+      // Pass the config
+      config);
+
+  frc2::RamseteCommand ramseteCommand(
+      exampleTrajectory, [this] { return m_drive.GetPose(); },
+      frc::RamseteController(AutoConstants::kRamseteB,
+                             AutoConstants::kRamseteZeta),
+      frc::SimpleMotorFeedforward<units::meters>(
+          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
+      DriveConstants::kDriveKinematics,
+      [this] { return m_drive.GetWheelSpeeds(); },
+      frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+      frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+      [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
+      {&m_drive});
+
+  // Reset odometry to starting pose of trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
+  // no auto
+  return new frc2::SequentialCommandGroup(
+      std::move(ramseteCommand),
+      frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
+}

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..d8cc462
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "subsystems/DriveSubsystem.h"
+
+#include <frc/RobotController.h>
+#include <frc/SPI.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem() {
+  // Set the distance per pulse for the encoders
+  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+
+  ResetEncoders();
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+  m_odometry.Update(m_gyro.GetRotation2d(),
+                    units::meter_t(m_leftEncoder.GetDistance()),
+                    units::meter_t(m_rightEncoder.GetDistance()));
+}
+
+void DriveSubsystem::SimulationPeriodic() {
+  // To update our simulation, we set motor voltage inputs, update the
+  // simulation, and write the simulated positions and velocities to our
+  // simulated encoder and gyro. We negate the right side so that positive
+  // voltages make the right side move forward.
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftMotors.Get()} *
+                                      frc::RobotController::GetInputVoltage(),
+                                  units::volt_t{-m_rightMotors.Get()} *
+                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.Update(20_ms);
+
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kLeftPosition));
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity));
+  m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kRightPosition));
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState(
+      frc::sim::DifferentialDrivetrainSim::State::kRightVelocity));
+  m_gyroAngleSim.SetAngle(
+      -m_drivetrainSimulator.GetHeading().Degrees().to<double>());
+
+  m_fieldSim.SetRobotPose(m_odometry.GetPose());
+}
+
+units::ampere_t DriveSubsystem::GetCurrentDraw() const {
+  return m_drivetrainSimulator.GetCurrentDraw();
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
+  m_leftMotors.SetVoltage(left);
+  m_rightMotors.SetVoltage(-right);
+  m_drive.Feed();
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}
+
+units::degree_t DriveSubsystem::GetHeading() const {
+  return m_gyro.GetRotation2d().Degrees();
+}
+
+double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
+  return {units::meters_per_second_t(m_leftEncoder.GetRate()),
+          units::meters_per_second_t(m_rightEncoder.GetRate())};
+}
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+  ResetEncoders();
+  m_drivetrainSimulator.SetPose(pose);
+  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+}

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
new file mode 100644
index 0000000..77e1184
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+#include <wpi/math>
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr auto kTrackwidth = 0.69_m;
+extern const frc::DifferentialDriveKinematics kDriveKinematics;
+
+constexpr int kEncoderCPR = 1024;
+constexpr auto kWheelDiameter = 6_in;
+constexpr double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameter.to<double>() * wpi::math::pi) /
+    static_cast<double>(kEncoderCPR);
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The Robot Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+constexpr auto ks = 0.22_V;
+constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
+
+constexpr auto kvAngular = 1.5 * 1_V * 1_s / 1_rad;
+constexpr auto kaAngular = 0.3 * 1_V * 1_s * 1_s / 1_rad;
+
+extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant;
+
+// Example values only -- use what's on your physical robot!
+constexpr auto kDrivetrainGearbox = frc::DCMotor::CIM(2);
+constexpr auto kDrivetrainGearing = 8.0;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPDriveVel = 8.5;
+}  // namespace DriveConstants
+
+namespace AutoConstants {
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
+
+// Reasonable baseline values for a RAMSETE follower in units of meters and
+// seconds
+constexpr double kRamseteB = 2;
+constexpr double kRamseteZeta = 0.7;
+}  // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h
new file mode 100644
index 0000000..5050999
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestPeriodic() override;
+  void SimulationPeriodic() override;
+
+ private:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+
+  RobotContainer m_container;
+};

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
new file mode 100644
index 0000000..06cbf7c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls).  Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+  RobotContainer();
+
+  frc2::Command* GetAutonomousCommand();
+  const DriveSubsystem& GetRobotDrive() const;
+
+ private:
+  // The driver's controller
+  frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+  // The robot's subsystems and commands are defined here...
+
+  // The robot's subsystems
+  DriveSubsystem m_drive;
+
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
+                                        {}};
+  frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+                                        {}};
+
+  // The chooser for the autonomous routines
+  frc::SendableChooser<frc2::Command*> m_chooser;
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..2cbc990
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/Field2d.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/voltage.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+  DriveSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  /**
+   * Will be called periodically during simulation.
+   */
+  void SimulationPeriodic() override;
+
+  // Subsystem methods go here.
+
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  void ArcadeDrive(double fwd, double rot);
+
+  /**
+   * Controls each side of the drive directly with a voltage.
+   *
+   * @param left the commanded left output
+   * @param right the commanded right output
+   */
+  void TankDriveVolts(units::volt_t left, units::volt_t right);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  double GetAverageEncoderDistance();
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  frc::Encoder& GetLeftEncoder();
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  frc::Encoder& GetRightEncoder();
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive
+   * more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  void SetMaxOutput(double maxOutput);
+
+  /**
+   * Returns the heading of the robot.
+   *
+   * @return the robot's heading in degrees, from -180 to 180
+   */
+  units::degree_t GetHeading() const;
+
+  /**
+   * Returns the turn rate of the robot.
+   *
+   * @return The turn rate of the robot, in degrees per second
+   */
+  double GetTurnRate();
+
+  /**
+   * Returns the currently-estimated pose of the robot.
+   *
+   * @return The pose.
+   */
+  frc::Pose2d GetPose();
+
+  /**
+   * Returns the current wheel speeds of the robot.
+   *
+   * @return The current wheel speeds.
+   */
+  frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
+
+  /**
+   * Resets the odometry to the specified pose.
+   *
+   * @param pose The pose to which to set the odometry.
+   */
+  void ResetOdometry(frc::Pose2d pose);
+
+ private:
+  // Components (e.g. motor controllers and sensors) should generally be
+  // declared private and exposed only through public methods.
+
+  // The motor controllers
+  frc::PWMVictorSPX m_left1{DriveConstants::kLeftMotor1Port};
+  frc::PWMVictorSPX m_left2{DriveConstants::kLeftMotor2Port};
+  frc::PWMVictorSPX m_right1{DriveConstants::kRightMotor1Port};
+  frc::PWMVictorSPX m_right2{DriveConstants::kRightMotor2Port};
+
+  // The motors on the left side of the drive
+  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+  // The motors on the right side of the drive
+  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+  // The robot's drive
+  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+  // The left-side drive encoder
+  frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
+                             DriveConstants::kLeftEncoderPorts[1]};
+
+  // The right-side drive encoder
+  frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
+                              DriveConstants::kRightEncoderPorts[1]};
+
+  // The gyro sensor
+  frc::AnalogGyro m_gyro{0};
+
+  // Odometry class for tracking robot pose
+  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+
+  // These classes help simulate our drivetrain.
+  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
+      DriveConstants::kDrivetrainPlant, DriveConstants::kTrackwidth,
+      DriveConstants::kDrivetrainGearbox, DriveConstants::kDrivetrainGearing,
+      DriveConstants::kWheelDiameter / 2};
+
+  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
+  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
+  frc::sim::AnalogGyroSim m_gyroAngleSim{m_gyro};
+
+  // The Field2d class simulates the field in the sim GUI. Note that we can have
+  // only one instance!
+  frc::Field2d m_fieldSim;
+};

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

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
new file mode 100644
index 0000000..e0eda4c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/Encoder.h>
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/LinearQuadraticRegulator.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/estimator/KalmanFilter.h>
+#include <frc/system/LinearSystemLoop.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/velocity.h>
+#include <wpi/math>
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control an elevator.
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr int kMotorPort = 0;
+  static constexpr int kEncoderAChannel = 0;
+  static constexpr int kEncoderBChannel = 1;
+  static constexpr int kJoystickPort = 0;
+
+  static constexpr units::meter_t kRaisedPosition = 2_ft;
+  static constexpr units::meter_t kLoweredPosition = 0_ft;
+
+  static constexpr units::meter_t kDrumRadius = 0.75_in;
+  static constexpr units::kilogram_t kCarriageMass = 4.5_kg;
+  static constexpr double kGearRatio = 6.0;
+
+  // The plant holds a state-space model of our elevator. This system has the
+  // following properties:
+  //
+  // States: [position, velocity], in meters and meters per second.
+  // Inputs (what we can "put in"): [voltage], in volts.
+  // Outputs (what we can measure): [position], in meters.
+  frc::LinearSystem<2, 1, 1> m_elevatorPlant =
+      frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), kCarriageMass,
+                                          kDrumRadius, kGearRatio);
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  frc::KalmanFilter<2, 1, 1> m_observer{
+      m_elevatorPlant,
+      {0.0508, 0.5},  // How accurate we think our model is
+      {0.001},        // How accurate we think our encoder position
+      // data is. In this case we very highly trust our encoder position
+      // reading.
+      20_ms};
+
+  // A LQR uses feedback to create voltage commands.
+  frc::LinearQuadraticRegulator<2, 1> m_controller{
+      m_elevatorPlant,
+      // qelms. State error tolerance, in meters and meters per second.
+      // Decrease this to more heavily penalize state excursion, or make the
+      // controller behave more aggressively.
+      {0.0254, 0.254},
+      // relms. Control effort (voltage) tolerance. Decrease this to more
+      // heavily penalize control effort, or make the controller less
+      // aggressive. 12 is a good starting point because that is the
+      // (approximate) maximum voltage of a battery.
+      {12.0},
+      // Nominal time between loops. 20ms for TimedRobot, but can be lower if
+      // using notifiers.
+      20_ms};
+
+  // The state-space loop combines a controller, observer, feedforward and plant
+  // for easy control.
+  frc::LinearSystemLoop<2, 1, 1> m_loop{m_elevatorPlant, m_controller,
+                                        m_observer, 12_V, 20_ms};
+
+  // An encoder set up to measure elevator height in meters.
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+
+  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::XboxController m_joystick{kJoystickPort};
+
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{3_fps,
+                                                                  6_fps_sq};
+
+  frc::TrapezoidProfile<units::meters>::State m_lastProfiledReference;
+
+ public:
+  void RobotInit() {
+    // Circumference = pi * d, so distance per click = pi * d / counts
+    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi *
+                                  kDrumRadius.to<double>() / 4096.0);
+  }
+
+  void TeleopInit() {
+    // Reset our loop to make sure it's in a known state.
+    m_loop.Reset(
+        frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate()));
+
+    m_lastProfiledReference = {units::meter_t(m_encoder.GetDistance()),
+                               units::meters_per_second_t(m_encoder.GetRate())};
+  }
+
+  void TeleopPeriodic() {
+    // Sets the target height of our elevator. This is similar to setting the
+    // setpoint of a PID controller.
+    frc::TrapezoidProfile<units::meters>::State goal;
+    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+      // We pressed the bumper, so let's set our next reference
+      goal = {kRaisedPosition, 0_fps};
+    } else {
+      // We released the bumper, so let's spin down
+      goal = {kLoweredPosition, 0_fps};
+    }
+    m_lastProfiledReference =
+        (frc::TrapezoidProfile<units::meters>(m_constraints, goal,
+                                              m_lastProfiledReference))
+            .Calculate(20_ms);
+
+    m_loop.SetNextR(
+        frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to<double>(),
+                              m_lastProfiledReference.velocity.to<double>()));
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to
+    // predict the next state with out Kalman filter.
+    m_loop.Predict(20_ms);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    m_motor.SetVoltage(units::volt_t(m_loop.U(0)));
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

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

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
new file mode 100644
index 0000000..ca3d909
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DriverStation.h>
+#include <frc/Encoder.h>
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/LinearQuadraticRegulator.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/estimator/KalmanFilter.h>
+#include <frc/system/LinearSystemLoop.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angular_velocity.h>
+#include <wpi/math>
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control a flywheel.
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr int kMotorPort = 0;
+  static constexpr int kEncoderAChannel = 0;
+  static constexpr int kEncoderBChannel = 1;
+  static constexpr int kJoystickPort = 0;
+  static constexpr units::radians_per_second_t kSpinup = 500_rpm;
+
+  static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
+      0.00032_kg_sq_m;
+
+  // Reduction between motors and encoder, as output over input. If the flywheel
+  // spins slower than the motors, this number should be greater than one.
+  static constexpr double kFlywheelGearing = 1.0;
+
+  // The plant holds a state-space model of our flywheel. This system has the
+  // following properties:
+  //
+  // States: [velocity], in radians per second.
+  // Inputs (what we can "put in"): [voltage], in volts.
+  // Outputs (what we can measure): [velocity], in radians per second.
+  frc::LinearSystem<1, 1, 1> m_flywheelPlant =
+      frc::LinearSystemId::FlywheelSystem(
+          frc::DCMotor::NEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  frc::KalmanFilter<1, 1, 1> m_observer{
+      m_flywheelPlant,
+      {3.0},   // How accurate we think our model is
+      {0.01},  // How accurate we think our encoder data is
+      20_ms};
+
+  // A LQR uses feedback to create voltage commands.
+  frc::LinearQuadraticRegulator<1, 1> m_controller{
+      m_flywheelPlant,
+      // qelms. Velocity error tolerance, in radians per second. Decrease this
+      // to more heavily penalize state excursion, or make the controller behave
+      // more aggressively.
+      {8.0},
+      // relms. Control effort (voltage) tolerance. Decrease this to more
+      // heavily penalize control effort, or make the controller less
+      // aggressive. 12 is a good starting point because that is the
+      // (approximate) maximum voltage of a battery.
+      {12.0},
+      // Nominal time between loops. 20ms for TimedRobot, but can be lower if
+      // using notifiers.
+      20_ms};
+
+  // The state-space loop combines a controller, observer, feedforward and plant
+  // for easy control.
+  frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller,
+                                        m_observer, 12_V, 20_ms};
+
+  // An encoder set up to measure flywheel velocity in radians per second.
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+
+  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::XboxController m_joystick{kJoystickPort};
+
+ public:
+  void RobotInit() {
+    // We go 2 pi radians per 4096 clicks.
+    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0);
+  }
+
+  void TeleopInit() {
+    m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+  }
+
+  void TeleopPeriodic() {
+    // Sets the target speed of our flywheel. This is similar to setting the
+    // setpoint of a PID controller.
+    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+      // We pressed the bumper, so let's set our next reference
+      m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
+    } else {
+      // We released the bumper, so let's spin down
+      m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0));
+    }
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to
+    // predict the next state with out Kalman filter.
+    m_loop.Predict(20_ms);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    m_motor.SetVoltage(units::volt_t(m_loop.U(0)));
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

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

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
new file mode 100644
index 0000000..02883fb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DriverStation.h>
+#include <frc/Encoder.h>
+#include <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/LinearPlantInversionFeedforward.h>
+#include <frc/controller/LinearQuadraticRegulator.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/estimator/KalmanFilter.h>
+#include <frc/system/LinearSystemLoop.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <wpi/math>
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control a flywheel.
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr int kMotorPort = 0;
+  static constexpr int kEncoderAChannel = 0;
+  static constexpr int kEncoderBChannel = 1;
+  static constexpr int kJoystickPort = 0;
+  static constexpr units::radians_per_second_t kSpinup = 500_rpm;
+
+  // Volts per (radian per second)
+  static constexpr auto kFlywheelKv = 0.023_V / 1_rad_per_s;
+
+  // Volts per (radian per second squared)
+  static constexpr auto kFlywheelKa = 0.001_V / 1_rad_per_s_sq;
+
+  // The plant holds a state-space model of our flywheel. This system has the
+  // following properties:
+  //
+  // States: [velocity], in radians per second.
+  // Inputs (what we can "put in"): [voltage], in volts.
+  // Outputs (what we can measure): [velocity], in radians per second.
+  //
+  // The Kv and Ka constants are found using the FRC Characterization toolsuite.
+  frc::LinearSystem<1, 1, 1> m_flywheelPlant =
+      frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(kFlywheelKv,
+                                                                 kFlywheelKa);
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  frc::KalmanFilter<1, 1, 1> m_observer{
+      m_flywheelPlant,
+      {3.0},   // How accurate we think our model is
+      {0.01},  // How accurate we think our encoder data is
+      20_ms};
+
+  // A LQR uses feedback to create voltage commands.
+  frc::LinearQuadraticRegulator<1, 1> m_controller{
+      m_flywheelPlant,
+      // qelms. Velocity error tolerance, in radians per second. Decrease this
+      // to more heavily penalize state excursion, or make the controller behave
+      // more aggressively.
+      {8.0},
+      // relms. Control effort (voltage) tolerance. Decrease this to more
+      // heavily penalize control effort, or make the controller less
+      // aggressive. 12 is a good starting point because that is the
+      // (approximate) maximum voltage of a battery.
+      {12.0},
+      // Nominal time between loops. 20ms for TimedRobot, but can be lower if
+      // using notifiers.
+      20_ms};
+
+  // The state-space loop combines a controller, observer, feedforward and plant
+  // for easy control.
+  frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller,
+                                        m_observer, 12_V, 20_ms};
+
+  // An encoder set up to measure flywheel velocity in radians per second.
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+
+  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::XboxController m_joystick{kJoystickPort};
+
+ public:
+  void RobotInit() {
+    // We go 2 pi radians per 4096 clicks.
+    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0);
+  }
+
+  void TeleopInit() {
+    m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+  }
+
+  void TeleopPeriodic() {
+    // Sets the target speed of our flywheel. This is similar to setting the
+    // setpoint of a PID controller.
+    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+      // We pressed the bumper, so let's set our next reference
+      m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
+    } else {
+      // We released the bumper, so let's spin down
+      m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0));
+    }
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to
+    // predict the next state with out Kalman filter.
+    m_loop.Predict(20_ms);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    m_motor.SetVoltage(units::volt_t(m_loop.U(0)));
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
index 416d303..fcc0bcd 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
                        units::radians_per_second_t rot, bool fieldRelative) {
   auto states = m_kinematics.ToSwerveModuleStates(
       fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                          xSpeed, ySpeed, rot, GetAngle())
+                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
 
   m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
@@ -26,6 +26,7 @@
 }
 
 void Drivetrain::UpdateOdometry() {
-  m_odometry.Update(GetAngle(), m_frontLeft.GetState(), m_frontRight.GetState(),
-                    m_backLeft.GetState(), m_backRight.GetState());
+  m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
+                    m_frontRight.GetState(), m_backLeft.GetState(),
+                    m_backRight.GetState());
 }

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

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
index 7376e9e..10e550b 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
@@ -1,10 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <frc/SlewRateLimiter.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 
@@ -23,23 +24,32 @@
   frc::XboxController m_controller{0};
   Drivetrain m_swerve;
 
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
   void DriveWithJoystick(bool fieldRelative) {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    const auto xSpeed =
-        -m_controller.GetY(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+    const auto xSpeed = -m_xspeedLimiter.Calculate(
+                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
-    const auto ySpeed =
-        -m_controller.GetX(frc::GenericHID::kLeftHand) * Drivetrain::kMaxSpeed;
+    const auto ySpeed = -m_yspeedLimiter.Calculate(
+                            m_controller.GetX(frc::GenericHID::kLeftHand)) *
+                        Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    const auto rot = -m_controller.GetX(frc::GenericHID::kRightHand) *
+    const auto rot = -m_rotLimiter.Calculate(
+                         m_controller.GetX(frc::GenericHID::kRightHand)) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);

wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index f7a11b8..720d554 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -40,11 +40,16 @@
   const auto driveOutput = m_drivePIDController.Calculate(
       m_driveEncoder.GetRate(), state.speed.to<double>());
 
+  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
+
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
       units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
 
+  const auto turnFeedforward = m_turnFeedforward.Calculate(
+      m_turningPIDController.GetSetpoint().velocity);
+
   // Set the motor outputs.
-  m_driveMotor.Set(driveOutput);
-  m_turningMotor.Set(turnOutput);
+  m_driveMotor.SetVoltage(units::volt_t{driveOutput} + driveFeedforward);
+  m_turningMotor.SetVoltage(units::volt_t{turnOutput} + turnFeedforward);
 }

wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
index 2de6e95..fed7ada 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,14 +22,6 @@
  public:
   Drivetrain() { m_gyro.Reset(); }
 
-  /**
-   * Get the robot angle as a Rotation2d.
-   */
-  frc::Rotation2d GetAngle() const {
-    // Negating the angle because WPILib Gyros are CW positive.
-    return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
-  }
-
   void Drive(units::meters_per_second_t xSpeed,
              units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
              bool fieldRelative);
@@ -57,5 +49,5 @@
       m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
       m_backRightLocation};
 
-  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()};
+  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, m_gyro.GetRotation2d()};
 };

wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
index 0dafa24..bf82df4 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,12 @@
 #include <frc/PWMVictorSPX.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/SwerveModuleState.h>
+#include <units/angular_velocity.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 class SwerveModule {
@@ -41,4 +46,9 @@
       0.0,
       0.0,
       {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
+
+  frc::SimpleMotorFeedforward<units::meters> m_driveFeedforward{1_V,
+                                                                3_V / 1_mps};
+  frc::SimpleMotorFeedforward<units::radians> m_turnFeedforward{
+      1_V, 0.5_V / 1_rad_per_s};
 };

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

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
index 44efba5..4bb69bb 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,7 +16,8 @@
 #include <frc2/command/SequentialCommandGroup.h>
 #include <frc2/command/SwerveControllerCommand.h>
 #include <frc2/command/button/JoystickButton.h>
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/velocity.h>
 
 #include "Constants.h"
 #include "subsystems/DriveSubsystem.h"
@@ -63,21 +64,28 @@
       // Pass the config
       config);
 
+  frc::ProfiledPIDController<units::radians> thetaController{
+      AutoConstants::kPThetaController, 0, 0,
+      AutoConstants::kThetaControllerConstraints};
+
+  thetaController.EnableContinuousInput(units::radian_t(-wpi::math::pi),
+                                        units::radian_t(wpi::math::pi));
+
   frc2::SwerveControllerCommand<4> swerveControllerCommand(
       exampleTrajectory, [this]() { return m_drive.GetPose(); },
 
       m_drive.kDriveKinematics,
 
       frc2::PIDController(AutoConstants::kPXController, 0, 0),
-      frc2::PIDController(AutoConstants::kPYController, 0, 0),
-      frc::ProfiledPIDController<units::radians>(
-          AutoConstants::kPThetaController, 0, 0,
-          AutoConstants::kThetaControllerConstraints),
+      frc2::PIDController(AutoConstants::kPYController, 0, 0), thetaController,
 
       [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
 
       {&m_drive});
 
+  // Reset odometry to the starting pose of the trajectory.
+  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(swerveControllerCommand), std::move(swerveControllerCommand),

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
index f5d05ce..37bb83d 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,9 @@
 #include "subsystems/DriveSubsystem.h"
 
 #include <frc/geometry/Rotation2d.h>
-#include <units/units.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/velocity.h>
 
 #include "Constants.h"
 
@@ -37,15 +39,13 @@
           kRearRightDriveEncoderPorts,    kRearRightTurningEncoderPorts,
           kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed},
 
-      m_odometry{kDriveKinematics,
-                 frc::Rotation2d(units::degree_t(GetHeading())),
-                 frc::Pose2d()} {}
+      m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d()} {}
 
 void DriveSubsystem::Periodic() {
   // Implementation of subsystem periodic method goes here.
-  m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
-                    m_frontLeft.GetState(), m_rearLeft.GetState(),
-                    m_frontRight.GetState(), m_rearRight.GetState());
+  m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
+                    m_rearLeft.GetState(), m_frontRight.GetState(),
+                    m_rearRight.GetState());
 }
 
 void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
@@ -54,8 +54,7 @@
                            bool fieldRelative) {
   auto states = kDriveKinematics.ToSwerveModuleStates(
       fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                          xSpeed, ySpeed, rot,
-                          frc::Rotation2d(units::degree_t(GetHeading())))
+                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
                     : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
 
   kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed);
@@ -85,15 +84,13 @@
   m_rearRight.ResetEncoders();
 }
 
-double DriveSubsystem::GetHeading() {
-  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+units::degree_t DriveSubsystem::GetHeading() const {
+  return m_gyro.GetRotation2d().Degrees();
 }
 
 void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
 
-double DriveSubsystem::GetTurnRate() {
-  return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
-}
+double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
 
 frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
 

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
index 51b7030..8911b5f 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,13 @@
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <units/units.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 #include <wpi/math>
 
 #pragma once
@@ -53,8 +59,6 @@
 constexpr bool kFrontRightDriveEncoderReversed = false;
 constexpr bool kRearRightDriveEncoderReversed = true;
 
-constexpr bool kGyroReversed = false;
-
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
 // theoretically for *your* robot's drive. The RobotPy Characterization

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
index db4623f..562af50 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -63,7 +63,7 @@
    *
    * @return the robot's heading in degrees, from 180 to 180
    */
-  double GetHeading();
+  units::degree_t GetHeading() const;
 
   /**
    * Zeroes the heading of the robot.

wpilibcExamples/src/main/cpp/examples/examples.json

diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json
index 4aa9f0e..93bfad4 100644
--- a/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -10,7 +10,7 @@
     ],
     "foldername": "MotorControl",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Motor Control With Encoder",
@@ -25,7 +25,7 @@
     ],
     "foldername": "MotorControlEncoder",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Relay",
@@ -37,7 +37,7 @@
     ],
     "foldername": "Relay",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "PDP CAN Monitoring",
@@ -49,7 +49,7 @@
     ],
     "foldername": "CANPDP",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Solenoids",
@@ -62,7 +62,7 @@
     ],
     "foldername": "Solenoid",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Encoder",
@@ -74,7 +74,7 @@
     ],
     "foldername": "Encoder",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Arcade Drive",
@@ -87,7 +87,7 @@
     ],
     "foldername": "ArcadeDrive",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Mecanum Drive",
@@ -100,7 +100,7 @@
     ],
     "foldername": "MecanumDrive",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Ultrasonic",
@@ -113,7 +113,7 @@
     ],
     "foldername": "Ultrasonic",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "UltrasonicPID",
@@ -126,7 +126,7 @@
     ],
     "foldername": "UltrasonicPID",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro",
@@ -140,7 +140,7 @@
     ],
     "foldername": "Gyro",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro Mecanum",
@@ -150,11 +150,11 @@
       "Complete List",
       "Sensors",
       "Analog",
-      "Joysitck"
+      "Joystick"
     ],
     "foldername": "GyroMecanum",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "HID Rumble",
@@ -164,7 +164,7 @@
     ],
     "foldername": "HidRumble",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "PotentiometerPID",
@@ -178,7 +178,7 @@
     ],
     "foldername": "PotentiometerPID",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with trapezoid profiled PID",
@@ -191,7 +191,7 @@
     ],
     "foldername": "ElevatorTrapezoidProfile",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with profiled PID controller",
@@ -204,7 +204,7 @@
     ],
     "foldername": "ElevatorProfiledPID",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Getting Started",
@@ -215,7 +215,7 @@
     ],
     "foldername": "GettingStarted",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Simple Vision",
@@ -226,7 +226,7 @@
     ],
     "foldername": "QuickVision",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Intermediate Vision",
@@ -237,7 +237,7 @@
     ],
     "foldername": "IntermediateVision",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Axis Camera Sample",
@@ -248,7 +248,7 @@
     ],
     "foldername": "AxisCameraSample",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "GearsBot",
@@ -280,7 +280,7 @@
     ],
     "foldername": "HAL",
     "gradlebase": "c",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "ShuffleBoard",
@@ -290,11 +290,11 @@
     ],
     "foldername": "ShuffleBoard",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "'Traditional' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'traditional' style, i.e. commands are given their own classes.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'traditional' style, i.e. commands are given their own classes.",
     "tags": [
       "Complete robot",
       "Command-based"
@@ -305,7 +305,7 @@
   },
   {
     "name": "'Inlined' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete robot",
       "Command-based",
@@ -317,7 +317,7 @@
   },
   {
     "name": "Select Command Example",
-    "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.",
+    "description": "An example showing how to use the SelectCommand class from the new command framework.",
     "tags": [
       "Command-based"
     ],
@@ -327,7 +327,7 @@
   },
   {
     "name": "Scheduler Event Logging",
-    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite",
+    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the new command framework",
     "tags": [
       "Command-based",
       "Shuffleboard"
@@ -414,7 +414,7 @@
     ],
     "foldername": "ArcadeDriveXboxController",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Tank Drive Xbox Controller",
@@ -427,7 +427,7 @@
     ],
     "foldername": "TankDriveXboxController",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Duty Cycle Encoder",
@@ -532,5 +532,134 @@
     "foldername": "DriveDistanceOffboard",
     "gradlebase": "cpp",
     "commandversion": 2
+  },
+  {
+    "name": "RamseteController",
+    "description": "An example robot demonstrating the use of RamseteController.",
+    "tags": [
+      "RamseteController"
+    ],
+    "foldername": "RamseteController",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceFlywheel",
+    "description": "An example state-space controller for a flywheel.",
+    "tags": [
+      "StateSpaceFlywheel",
+      "Flywheel",
+      "State Space",
+      "Model",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "StateSpaceFlywheel",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceFlywheelSysId",
+    "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.",
+    "tags": [
+      "StateSpaceFlywheelSysId",
+      "FRC Characterization",
+      "Flywheel",
+      "Characterization",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "StateSpaceFlywheelSysId",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceElevator",
+    "description": "An example state-space controller for controlling an elevator.",
+    "tags": [
+      "Elevator",
+      "State Space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "StateSpaceElevator",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceArm",
+    "description": "An example state-space controller for controlling an arm.",
+    "tags": [
+      "Arm",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "StateSpaceArm",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "ElevatorSimulation",
+    "description": "Demonstrates the use of physics simulation with a simple elevator.",
+    "tags": [
+      "Elevator",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Simulation",
+      "Physics"
+    ],
+    "foldername": "ElevatorSimulation",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "ArmSimulation",
+    "description": "Demonstrates the use of physics simulation with a simple single-jointed arm.",
+    "tags": [
+      "Arm",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Simulation",
+      "Physics"
+    ],
+    "foldername": "ArmSimulation",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceDriveSimulation",
+    "description": "Demonstrates the use of physics simulation with a differential drivetrain and the Field2d class.",
+    "tags": [
+      "Differential Drive",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Simulation",
+      "Physics",
+      "Drivetrain",
+      "Field2d"
+    ],
+    "foldername": "StateSpaceDifferentialDriveSimulation",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]

wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp

diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
index 2e720c9..0cfda95 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,3 +14,7 @@
 void ExampleSubsystem::Periodic() {
   // Implementation of subsystem periodic method goes here.
 }
+
+void ExampleSubsystem::SimulationPeriodic() {
+  // Implementation of subsystem simulation periodic method goes here.
+}

wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h

diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
index f6cd550..b64c26f 100644
--- a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,6 +18,12 @@
    */
   void Periodic() override;
 
+  /**
+   * Will be called periodically whenever the CommandScheduler runs during
+   * simulation.
+   */
+  void SimulationPeriodic() override;
+
  private:
   // Components (e.g. motor controllers and sensors) should generally be
   // declared private and exposed only through public methods.

wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp

diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
index de8fc3e..4bfae1f 100644
--- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -41,7 +41,7 @@
       m_ds.InAutonomous(true);
       Autonomous();
       m_ds.InAutonomous(false);
-      while (IsAutonomous() && IsEnabled()) m_ds.WaitForData();
+      while (IsAutonomousEnabled()) m_ds.WaitForData();
     } else if (IsTest()) {
       lw.SetEnabled(true);
       frc::Shuffleboard::EnableActuatorWidgets();
@@ -55,7 +55,7 @@
       m_ds.InOperatorControl(true);
       Teleop();
       m_ds.InOperatorControl(false);
-      while (IsOperatorControl() && IsEnabled()) m_ds.WaitForData();
+      while (IsOperatorControlEnabled()) m_ds.WaitForData();
     }
   }
 }

wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp

diff --git a/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
index 07d843d..dfb6993 100644
--- a/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -63,6 +63,12 @@
 
 void Robot::TeleopPeriodic() {}
 
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+void Robot::TestInit() {}
+
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS

wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h

diff --git a/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
index 09183dc..fe4ea1f 100644
--- a/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -20,6 +20,9 @@
   void AutonomousPeriodic() override;
   void TeleopInit() override;
   void TeleopPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void TestInit() override;
   void TestPeriodic() override;
 
  private:

wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp

diff --git a/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
index 76adfc4..425fa84 100644
--- a/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,7 @@
 #include "Robot.h"
 
 void Robot::RobotInit() {}
+void Robot::RobotPeriodic() {}
 
 void Robot::AutonomousInit() {}
 void Robot::AutonomousPeriodic() {}
@@ -15,6 +16,9 @@
 void Robot::TeleopInit() {}
 void Robot::TeleopPeriodic() {}
 
+void Robot::DisabledInit() {}
+void Robot::DisabledPeriodic() {}
+
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 

wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h

diff --git a/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
index bf4dae1..d4bcbb6 100644
--- a/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 class Robot : public frc::TimedRobot {
  public:
   void RobotInit() override;
+  void RobotPeriodic() override;
 
   void AutonomousInit() override;
   void AutonomousPeriodic() override;
@@ -19,6 +20,9 @@
   void TeleopInit() override;
   void TeleopPeriodic() override;
 
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+
   void TestInit() override;
   void TestPeriodic() override;
 };

wpilibcIntegrationTests/build.gradle

diff --git a/wpilibcIntegrationTests/build.gradle b/wpilibcIntegrationTests/build.gradle
index ed6d58e..81f827b 100644
--- a/wpilibcIntegrationTests/build.gradle
+++ b/wpilibcIntegrationTests/build.gradle
@@ -46,6 +46,7 @@
                     }
                     lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                     lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                     lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                     lib project: ':cscore', library: 'cscore', linkage: 'shared'
                     lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'

wpilibj/CMakeLists.txt

diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt
index 7874f7f..a07a563 100644
--- a/wpilibj/CMakeLists.txt
+++ b/wpilibj/CMakeLists.txt
@@ -3,7 +3,7 @@
 find_package( OpenCV REQUIRED )
 
 # Java bindings
-if (NOT WITHOUT_JAVA)
+if (WITH_JAVA)
     find_package(Java REQUIRED)
     include(UseJava)
     set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
@@ -15,17 +15,17 @@
     configure_file(src/generate/WPILibVersion.java.in WPILibVersion.java)
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-    file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar")
+    file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
     file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
 
-    add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpiutil_jar OUTPUT_NAME wpilibj)
+    add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiutil_jar OUTPUT_NAME wpilibj)
 
     get_property(WPILIBJ_JAR_FILE TARGET wpilibj_jar PROPERTY JAR_FILE)
     install(FILES ${WPILIBJ_JAR_FILE} DESTINATION "${java_lib_dest}")
 
     set_property(TARGET wpilibj_jar PROPERTY FOLDER "java")
 
-    if (MSVC OR FLAT_INSTALL_WPILIB)
+    if (WITH_FLAT_INSTALL)
         set (wpilibj_config_dir ${wpilib_dest})
     else()
         set (wpilibj_config_dir share/wpilibj)

wpilibj/build.gradle

diff --git a/wpilibj/build.gradle b/wpilibj/build.gradle
index b2e22fb..df16022 100644
--- a/wpilibj/build.gradle
+++ b/wpilibj/build.gradle
@@ -62,6 +62,7 @@
 dependencies {
     implementation project(':hal')
     implementation project(':wpiutil')
+    implementation project(':wpimath')
     implementation project(':ntcore')
     implementation project(':cscore')
     implementation project(':cameraserver')
@@ -69,6 +70,7 @@
     testImplementation 'org.mockito:mockito-core:2.27.0'
     devImplementation project(':hal')
     devImplementation project(':wpiutil')
+    devImplementation project(':wpimath')
     devImplementation project(':ntcore')
     devImplementation project(':cscore')
     devImplementation project(':cameraserver')
@@ -110,9 +112,11 @@
                 lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
                 lib project: ':cscore', library: 'cscore', linkage: 'shared'
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                 lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
                 lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
                 lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimathJNIShared', linkage: 'shared'
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 project(':hal').addHalDependency(it, 'shared')
                 project(':hal').addHalJniDependency(it)

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
index d901462..c31782c 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -98,7 +98,7 @@
     if (m_simDevice != null) {
       m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
       m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
-      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simY = m_simDevice.createDouble("Y Accel", false, 0.0);
       m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
     }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
index 6aed40e..842ffd6 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -89,7 +89,7 @@
     if (m_simDevice != null) {
       m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
       m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
-      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simY = m_simDevice.createDouble("Y Accel", false, 0.0);
       m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
     }
     init(range);

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
index 54583ea..340a776 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -97,7 +97,7 @@
     if (m_simDevice != null) {
       m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
       m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
-      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simY = m_simDevice.createDouble("Y Accel", false, 0.0);
       m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
     }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
index 549a91a..78ab8c4 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,7 +25,8 @@
  * instantiated, it does a short calibration routine where it samples the gyro while at rest to
  * determine the default offset. This is subtracted from each sample to determine the heading.
  *
- * <p>This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ * <p>This class is for the digital ADXRS450 gyro sensor that connects via SPI. Only one instance of
+ * an ADXRS Gyro is supported.
  */
 @SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
 public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable, AutoCloseable {
@@ -44,6 +45,7 @@
   private static final int kSNLowRegister = 0x10;
 
   private SPI m_spi;
+  private SPI.Port m_port;
 
   private SimDevice m_simDevice;
   private SimBoolean m_simConnected;
@@ -64,6 +66,7 @@
    */
   public ADXRS450_Gyro(SPI.Port port) {
     m_spi = new SPI(port);
+    m_port = port;
 
     // simulation
     m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value);
@@ -128,6 +131,15 @@
     m_spi.resetAccumulator();
   }
 
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  public int getPort() {
+    return m_port.value;
+  }
+
   private boolean calcParity(int value) {
     boolean parity = false;
     while (value != 0) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
index 223c6f2..5a333db 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,6 +14,8 @@
 
 /**
  * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ *
+ * <p>Only 1 LED driver is currently supported by the roboRIO.
  */
 public class AddressableLED implements AutoCloseable {
   private final int m_pwmHandle;
@@ -22,7 +24,7 @@
   /**
    * Constructs a new driver for a specific port.
    *
-   * @param port the output port to use (Must be a PWM port)
+   * @param port the output port to use (Must be a PWM header, not on MXP)
    */
   public AddressableLED(int port) {
     m_pwmHandle = PWMJNI.initializePWMPort(HAL.getPort((byte) port));
@@ -45,6 +47,8 @@
    *
    * <p>Calling this is an expensive call, so its best to call it once, then just update data.
    *
+   * <p>The max length is 5460 LEDs.
+   *
    * @param length the strip length
    */
   public void setLength(int length) {
@@ -94,7 +98,7 @@
   /**
    * Starts the output.
    *
-   * <p>The output writes continously.
+   * <p>The output writes continuously.
    */
   public void start() {
     AddressableLEDJNI.start(m_handle);

wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
index 3cc0763..8b76429 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -116,4 +116,28 @@
   public int getLength() {
     return m_buffer.length / 4;
   }
+
+  /**
+   * Gets the color at the specified index.
+   *
+   * @param index the index to get
+   * @return the LED color at the specified index
+   */
+  public Color8Bit getLED8Bit(int index) {
+    return new Color8Bit(m_buffer[index * 4 + 2] & 0xFF, m_buffer[index * 4 + 1] & 0xFF,
+                         m_buffer[index * 4] & 0xFF);
+  }
+
+  /**
+   * Gets the color at the specified index.
+   *
+   * @param index the index to get
+   * @return the LED color at the specified index
+   */
+  public Color getLED(int index) {
+    return new Color((m_buffer[index * 4 + 2] & 0xFF) / 255.0,
+                     (m_buffer[index * 4 + 1] & 0xFF) / 255.0,
+                     (m_buffer[index * 4] & 0xFF) / 255.0);
+  }
+
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
index 75177e0..bea4e6a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -133,6 +133,15 @@
   }
 
   /**
+   * Get the channel number.
+   *
+   * @return The channel number.
+   */
+  public int getChannel() {
+    return m_analogInput.getChannel();
+  }
+
+  /**
    * Reset the Encoder distance to zero.
    */
   public void reset() {

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
index c688b55..0ed22d9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -184,7 +184,16 @@
    *
    * @param volts The size of the deadband in volts
    */
-  void setDeadband(double volts) {
+  public void setDeadband(double volts) {
     AnalogGyroJNI.setAnalogGyroDeadband(m_gyroHandle, volts);
   }
+
+  /**
+   * Gets the analog input for the gyro.
+   *
+   * @return AnalogInput
+   */
+  public AnalogInput getAnalogInput() {
+    return m_analog;
+  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
index 308be6b..3298058 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,6 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.hal.sim.AnalogInSim;
 import edu.wpi.first.hal.util.AllocationException;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -360,8 +359,4 @@
     builder.setSmartDashboardType("Analog Input");
     builder.addDoubleProperty("Value", this::getAverageVoltage, null);
   }
-
-  public AnalogInSim getSimObject() {
-    return new AnalogInSim(m_channel);
-  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
index ae03463..feb6e1e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 import edu.wpi.first.hal.AnalogJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.sim.AnalogOutSim;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -65,8 +64,4 @@
     builder.setSmartDashboardType("Analog Output");
     builder.addDoubleProperty("Value", this::getVoltage, this::setVoltage);
   }
-
-  public AnalogOutSim getSimObject() {
-    return new AnalogOutSim(m_channel);
-  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
index 3a6f338..9d5d49d 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,7 +32,8 @@
    * point after scaling is 135 degrees. This will calculate the result from the fullRange times
    * the fraction of the supply voltage, plus the offset.
    *
-   * @param channel   The analog channel this potentiometer is plugged into.
+   * @param channel   The analog input channel this potentiometer is plugged into. 0-3 are
+   *                  on-board and 4-7 are on the MXP port.
    * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
    * @param offset    The offset to add to the scaled value for controlling the zero value
    */
@@ -46,14 +47,16 @@
    * AnalogPotentiometer constructor.
    *
    * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
-   * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
-   * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+   * have a 270 degree potentiometer and you want the output to be degrees with the halfway point
+   * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
    * point after scaling is 135 degrees. This will calculate the result from the fullRange times
    * the fraction of the supply voltage, plus the offset.
    *
    * @param input     The {@link AnalogInput} this potentiometer is plugged into.
-   * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
-   * @param offset    The offset to add to the scaled value for controlling the zero value
+   * @param fullRange The angular value (in desired units) representing the full
+   *                  0-5V range of the input.
+   * @param offset    The angular value (in desired units) representing the
+   *                  angular output at 0V.
    */
   public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
     SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel());
@@ -67,12 +70,12 @@
   /**
    * AnalogPotentiometer constructor.
    *
-   * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
-   * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
-   * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
-   * point after scaling is 135 degrees.
+   * <p>Use the scale value so that the output produces meaningful values. I.E: you
+   * have a 270 degree potentiometer and you want the output to be degrees with the starting point
+   * as 0 degrees. The scale value is 270.0(degrees).
    *
-   * @param channel The analog channel this potentiometer is plugged into.
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are
+   *                on-board and 4-7 are on the MXP port.
    * @param scale   The scaling to multiply the voltage by to get a meaningful unit.
    */
   public AnalogPotentiometer(final int channel, double scale) {
@@ -83,9 +86,8 @@
    * AnalogPotentiometer constructor.
    *
    * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
-   * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
-   * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
-   * point after scaling is 135 degrees.
+   * have a 270 degree potentiometer and you want the output to be degrees with the starting point
+   * as 0 degrees. The scale value is 270.0(degrees).
    *
    * @param input The {@link AnalogInput} this potentiometer is plugged into.
    * @param scale The scaling to multiply the voltage by to get a meaningful unit.
@@ -97,7 +99,10 @@
   /**
    * AnalogPotentiometer constructor.
    *
-   * @param channel The analog channel this potentiometer is plugged into.
+   * <p>The potentiometer will return a value between 0 and 1.0.
+   *
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are
+   *                  on-board and 4-7 are on the MXP port.
    */
   public AnalogPotentiometer(final int channel) {
     this(channel, 1, 0);
@@ -106,6 +111,8 @@
   /**
    * AnalogPotentiometer constructor.
    *
+   * <p>The potentiometer will return a value between 0 and 1.0.
+   *
    * @param input The {@link AnalogInput} this potentiometer is plugged into.
    */
   public AnalogPotentiometer(final AnalogInput input) {
@@ -122,7 +129,8 @@
     if (m_analogInput == null) {
       return m_offset;
     }
-    return (m_analogInput.getVoltage() / RobotController.getVoltage5V()) * m_fullRange + m_offset;
+    return (m_analogInput.getAverageVoltage() / RobotController.getVoltage5V())
+        * m_fullRange + m_offset;
   }
 
   @Override
@@ -151,7 +159,8 @@
   @Override
   public void initSendable(SendableBuilder builder) {
     if (m_analogInput != null) {
-      m_analogInput.initSendable(builder);
+      builder.setSmartDashboardType("Analog Input");
+      builder.addDoubleProperty("Value", this::get, null);
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
index f7f98bd..282a346 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -23,6 +23,7 @@
   /**
    * Exceptions dealing with improper operation of the Analog trigger.
    */
+  @SuppressWarnings("serial")
   public static class AnalogTriggerException extends RuntimeException {
     /**
      * Create a new exception with the given message.

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
index 7635196..f9fe950 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -44,6 +44,7 @@
   /**
    * Exceptions dealing with improper operation of the Analog trigger output.
    */
+  @SuppressWarnings("serial")
   public static class AnalogTriggerOutputException extends RuntimeException {
     /**
      * Create a new exception with the given message.

wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
index f6811c0..e367756 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,6 @@
 import edu.wpi.first.hal.AccelerometerJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.sim.AccelerometerSim;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -104,8 +103,4 @@
     builder.addDoubleProperty("Y", this::getY, null);
     builder.addDoubleProperty("Z", this::getZ, null);
   }
-
-  public AccelerometerSim getSimObject() {
-    return new AccelerometerSim();
-  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
index 7b188e5..2dde3ab 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -101,6 +101,39 @@
   }
 
   /**
+   * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+   *
+   * @param data The data to write (8 bytes max)
+   * @param apiId The API ID to write.
+   */
+  public int writePacketNoThrow(byte[] data, int apiId) {
+    return CANAPIJNI.writeCANPacketNoThrow(m_handle, data, apiId);
+  }
+
+  /**
+   * Write a repeating packet to the CAN device with a specific ID. This ID is 10 bits.
+   * The RoboRIO will automatically repeat the packet at the specified interval
+   *
+   * @param data The data to write (8 bytes max)
+   * @param apiId The API ID to write.
+   * @param repeatMs The period to repeat the packet at.
+   */
+  public int writePacketRepeatingNoThrow(byte[] data, int apiId, int repeatMs) {
+    return CANAPIJNI.writeCANPacketRepeatingNoThrow(m_handle, data, apiId, repeatMs);
+  }
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10 bits.
+   * The length by spec must match what is returned by the responding device
+   *
+   * @param length The length to request (0 to 8)
+   * @param apiId The API ID to write.
+   */
+  public int writeRTRFrameNoThrow(int length, int apiId) {
+    return CANAPIJNI.writeCANRTRFrameNoThrow(m_handle, length, apiId);
+  }
+
+  /**
    * Stop a repeating packet with a specific ID. This ID is 10 bits.
    *
    * @param apiId The API ID to stop repeating

wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
index 178d5ee..c8948b9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -39,7 +39,6 @@
  *
  * @deprecated Replaced with edu.wpi.first.cameraserver.CameraServer
  */
-@SuppressWarnings("PMD.TooManyMethods")
 @Deprecated
 public final class CameraServer {
   public static final int kBasePort = 1181;

wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
index 9d8a8dd..8fbefd8 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -196,6 +196,15 @@
     CompressorJNI.clearAllPCMStickyFaults(m_module);
   }
 
+  /**
+   * Gets the module number (CAN ID).
+   *
+   * @return Module number
+   */
+  public int getModule() {
+    return m_module;
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Compressor");

wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
index d612d24..d6d4908 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -133,11 +133,27 @@
 
     if (valueForward) {
       return Value.kForward;
-    }
-    if (valueReverse) {
+    } else if (valueReverse) {
       return Value.kReverse;
+    } else {
+      return Value.kOff;
     }
-    return Value.kOff;
+  }
+
+  /**
+   * Toggle the value of the solenoid.
+   *
+   * <p>If the solenoid is set to forward, it'll be set to reverse. If the solenoid is set to
+   * reverse, it'll be set to forward. If the solenoid is set to off, nothing happens.
+   */
+  public void toggle() {
+    Value value = get();
+
+    if (value == Value.kForward) {
+      set(Value.kReverse);
+    } else if (value == Value.kReverse) {
+      set(Value.kForward);
+    }
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
index 997fe2b..fe3ca50 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,8 +25,7 @@
  * Provide access to the network communication data to / from the Driver Station.
  */
 @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.ExcessiveClassLength",
-                   "PMD.ExcessivePublicCount", "PMD.GodClass", "PMD.TooManyFields",
-                   "PMD.TooManyMethods"})
+                   "PMD.ExcessivePublicCount", "PMD.GodClass", "PMD.TooManyFields"})
 public class DriverStation {
   /**
    * Number of Joystick Ports.
@@ -164,6 +163,9 @@
   private final Lock m_waitForDataMutex;
   private final Condition m_waitForDataCond;
   private int m_waitForDataCount;
+  private final ThreadLocal<Integer> m_lastCount = ThreadLocal.withInitial(() -> 0);
+
+  private boolean m_silenceJoystickWarning;
 
   // Robot state status variables
   private boolean m_userInDisabled;
@@ -316,21 +318,19 @@
       reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
       return false;
     }
+
     m_cacheDataMutex.lock();
     try {
-      if (button > m_joystickButtons[stick].m_count) {
-        // Unlock early so error printing isn't locked.
-        m_cacheDataMutex.unlock();
-        reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
-            + " not available, check if controller is plugged in");
+      if (button <= m_joystickButtons[stick].m_count) {
+        return (m_joystickButtons[stick].m_buttons & 1 << (button - 1)) != 0;
       }
-
-      return (m_joystickButtons[stick].m_buttons & 1 << (button - 1)) != 0;
     } finally {
-      if (m_cacheDataMutex.isHeldByCurrentThread()) {
-        m_cacheDataMutex.unlock();
-      }
+      m_cacheDataMutex.unlock();
     }
+
+    reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+        + " not available, check if controller is plugged in");
+    return false;
   }
 
   /**
@@ -340,7 +340,7 @@
    * @param button The button index, beginning at 1.
    * @return Whether the joystick button was pressed since the last check.
    */
-  boolean getStickButtonPressed(final int stick, final int button) {
+  public boolean getStickButtonPressed(final int stick, final int button) {
     if (button <= 0) {
       reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
       return false;
@@ -348,27 +348,25 @@
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
     }
-    boolean error = false;
-    boolean retVal = false;
-    synchronized (m_cacheDataMutex) {
-      if (button > m_joystickButtons[stick].m_count) {
-        error = true;
-        retVal = false;
-      } else {
+
+    m_cacheDataMutex.lock();
+    try {
+      if (button <= m_joystickButtons[stick].m_count) {
         // If button was pressed, clear flag and return true
         if ((m_joystickButtonsPressed[stick] & 1 << (button - 1)) != 0) {
           m_joystickButtonsPressed[stick] &= ~(1 << (button - 1));
-          retVal = true;
+          return true;
         } else {
-          retVal = false;
+          return false;
         }
       }
+    } finally {
+      m_cacheDataMutex.unlock();
     }
-    if (error) {
-      reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
-          + " not available, check if controller is plugged in");
-    }
-    return retVal;
+
+    reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+        + " not available, check if controller is plugged in");
+    return false;
   }
 
   /**
@@ -379,7 +377,7 @@
    * @param button The button index, beginning at 1.
    * @return Whether the joystick button was released since the last check.
    */
-  boolean getStickButtonReleased(final int stick, final int button) {
+  public boolean getStickButtonReleased(final int stick, final int button) {
     if (button <= 0) {
       reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
       return false;
@@ -387,27 +385,25 @@
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
     }
-    boolean error = false;
-    boolean retVal = false;
-    synchronized (m_cacheDataMutex) {
-      if (button > m_joystickButtons[stick].m_count) {
-        error = true;
-        retVal = false;
-      } else {
+
+    m_cacheDataMutex.lock();
+    try {
+      if (button <= m_joystickButtons[stick].m_count) {
         // If button was released, clear flag and return true
         if ((m_joystickButtonsReleased[stick] & 1 << (button - 1)) != 0) {
           m_joystickButtonsReleased[stick] &= ~(1 << (button - 1));
-          retVal = true;
+          return true;
         } else {
-          retVal = false;
+          return false;
         }
       }
+    } finally {
+      m_cacheDataMutex.unlock();
     }
-    if (error) {
-      reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
-          + " not available, check if controller is plugged in");
-    }
-    return retVal;
+
+    reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+        + " not available, check if controller is plugged in");
+    return false;
   }
 
   /**
@@ -428,20 +424,16 @@
 
     m_cacheDataMutex.lock();
     try {
-      if (axis >= m_joystickAxes[stick].m_count) {
-        // Unlock early so error printing isn't locked.
-        m_cacheDataMutex.unlock();
-        reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
-            + " not available, check if controller is plugged in");
-        return 0.0;
+      if (axis < m_joystickAxes[stick].m_count) {
+        return m_joystickAxes[stick].m_axes[axis];
       }
-
-      return m_joystickAxes[stick].m_axes[axis];
     } finally {
-      if (m_cacheDataMutex.isHeldByCurrentThread()) {
-        m_cacheDataMutex.unlock();
-      }
+      m_cacheDataMutex.unlock();
     }
+
+    reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
+        + " not available, check if controller is plugged in");
+    return 0.0;
   }
 
   /**
@@ -459,20 +451,16 @@
 
     m_cacheDataMutex.lock();
     try {
-      if (pov >= m_joystickPOVs[stick].m_count) {
-        // Unlock early so error printing isn't locked.
-        m_cacheDataMutex.unlock();
-        reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
-            + " not available, check if controller is plugged in");
-        return -1;
+      if (pov < m_joystickPOVs[stick].m_count) {
+        return m_joystickPOVs[stick].m_povs[pov];
       }
     } finally {
-      if (m_cacheDataMutex.isHeldByCurrentThread()) {
-        m_cacheDataMutex.unlock();
-      }
+      m_cacheDataMutex.unlock();
     }
 
-    return m_joystickPOVs[stick].m_povs[pov];
+    reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
+        + " not available, check if controller is plugged in");
+    return -1;
   }
 
   /**
@@ -609,6 +597,21 @@
   }
 
   /**
+   * Returns if a joystick is connected to the Driver Station.
+   *
+   * <p>This makes a best effort guess by looking at the reported number of axis,
+   * buttons, and POVs attached.
+   *
+   * @param stick The joystick port number
+   * @return true if a joystick is connected
+   */
+  public boolean isJoystickConnected(int stick) {
+    return getStickAxisCount(stick) > 0
+      || getStickButtonCount(stick) > 0
+      || getStickPOVCount(stick) > 0;
+  }
+
+  /**
    * Gets a value indicating whether the Driver Station requires the robot to be enabled.
    *
    * @return True if the robot is enabled, false otherwise.
@@ -656,6 +659,19 @@
 
   /**
    * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * autonomous mode and enabled.
+   *
+   * @return True if autonomous should be set and the robot should be enabled.
+   */
+  public boolean isAutonomousEnabled() {
+    synchronized (m_controlWordMutex) {
+      updateControlWord(false);
+      return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled();
+    }
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
    * operator-controlled mode.
    *
    * @return True if operator-controlled mode should be enabled, false otherwise.
@@ -665,6 +681,20 @@
   }
 
   /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controller mode and enabled.
+   *
+   * @return True if operator-controlled mode should be set and the robot should be enabled.
+   */
+  public boolean isOperatorControlEnabled() {
+    synchronized (m_controlWordMutex) {
+      updateControlWord(false);
+      return !m_controlWordCache.getAutonomous() && !m_controlWordCache.getTest()
+          && m_controlWordCache.getEnabled();
+    }
+  }
+
+  /**
    * Gets a value indicating whether the Driver Station requires the robot to be running in test
    * mode.
    *
@@ -696,7 +726,17 @@
    * @return True if the control data has been updated since the last call.
    */
   public boolean isNewControlData() {
-    return HAL.isNewControlData();
+    m_waitForDataMutex.lock();
+    try {
+      int currentCount = m_waitForDataCount;
+      if (m_lastCount.get() != currentCount) {
+        m_lastCount.set(currentCount);
+        return true;
+      }
+    } finally {
+      m_waitForDataMutex.unlock();
+    }
+    return false;
   }
 
   /**
@@ -849,6 +889,9 @@
 
   /**
    * Wait for new data from the driver station.
+   *
+   * <p>Checks if new control data has arrived since the last waitForData call
+   * on the current thread. If new data has not arrived, returns immediately.
    */
   public void waitForData() {
     waitForData(0);
@@ -858,6 +901,9 @@
    * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
    * only.
    *
+   * <p>Checks if new control data has arrived since the last waitForData call
+   * on the current thread. If new data has not arrived, returns immediately.
+   *
    * @param timeout The maximum time in seconds to wait.
    * @return true if there is new data, otherwise false
    */
@@ -867,6 +913,10 @@
     m_waitForDataMutex.lock();
     try {
       int currentCount = m_waitForDataCount;
+      if (m_lastCount.get() != currentCount) {
+        m_lastCount.set(currentCount);
+        return true;
+      }
       while (m_waitForDataCount == currentCount) {
         if (timeout > 0) {
           long now = RobotController.getFPGATime();
@@ -886,6 +936,7 @@
           m_waitForDataCond.await();
         }
       }
+      m_lastCount.set(m_waitForDataCount);
       // Return true if we have received a proper signal
       return true;
     } catch (InterruptedException ex) {
@@ -901,7 +952,7 @@
    * but does send an approximate match time. The value will count down the time remaining in the
    * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
    * dispute ref calls or guarantee that a function will trigger before the match ends) The
-   * Practice Match function of the DS approximates the behaviour seen on the field.
+   * Practice Match function of the DS approximates the behavior seen on the field.
    *
    * @return Time remaining in current match period (auto or teleop) in seconds
    */
@@ -1019,6 +1070,27 @@
   }
 
   /**
+   * Allows the user to specify whether they want joystick connection warnings
+   * to be printed to the console. This setting is ignored when the FMS is
+   * connected -- warnings will always be on in that scenario.
+   *
+   * @param silence Whether warning messages should be silenced.
+   */
+  public void silenceJoystickConnectionWarning(boolean silence) {
+    m_silenceJoystickWarning = silence;
+  }
+
+  /**
+   * Returns whether joystick connection warnings are silenced. This will
+   * always return false when connected to the FMS.
+   *
+   * @return Whether joystick connection warnings are silenced.
+   */
+  public boolean isJoystickConnectionWarningSilenced() {
+    return !isFMSAttached() && m_silenceJoystickWarning;
+  }
+
+  /**
    * Copy data from the DS task for the user. If no new data exists, it will just be returned,
    * otherwise the data will be copied from the DS polling loop.
    */
@@ -1092,10 +1164,12 @@
    * the DS.
    */
   private void reportJoystickUnpluggedWarning(String message) {
-    double currentTime = Timer.getFPGATimestamp();
-    if (currentTime > m_nextMessageTime) {
-      reportWarning(message, false);
-      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+    if (isFMSAttached() || !m_silenceJoystickWarning) {
+      double currentTime = Timer.getFPGATimestamp();
+      if (currentTime > m_nextMessageTime) {
+        reportWarning(message, false);
+        m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+      }
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
index b2040f4..f5bba5e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,6 +22,7 @@
 public class DutyCycleEncoder implements Sendable, AutoCloseable {
   private final DutyCycle m_dutyCycle;
   private boolean m_ownsDutyCycle;
+  private DigitalInput m_digitalInput;
   private AnalogTrigger m_analogTrigger;
   private Counter m_counter;
   private int m_frequencyThreshold = 100;
@@ -31,6 +32,7 @@
 
   protected SimDevice m_simDevice;
   protected SimDouble m_simPosition;
+  protected SimDouble m_simDistancePerRotation;
   protected SimBoolean m_simIsConnected;
 
   /**
@@ -39,7 +41,9 @@
    * @param channel the channel to attach to
    */
   public DutyCycleEncoder(int channel) {
-    m_dutyCycle = new DutyCycle(new DigitalInput(channel));
+    m_digitalInput = new DigitalInput(channel);
+    m_ownsDutyCycle = true;
+    m_dutyCycle = new DutyCycle(m_digitalInput);
     init();
   }
 
@@ -65,20 +69,20 @@
   }
 
   private void init() {
-    m_analogTrigger = new AnalogTrigger(m_dutyCycle);
-    m_counter = new Counter();
-
     m_simDevice = SimDevice.create("DutyCycleEncoder", m_dutyCycle.getFPGAIndex());
 
     if (m_simDevice != null) {
       m_simPosition = m_simDevice.createDouble("Position", false, 0.0);
+      m_simDistancePerRotation = m_simDevice.createDouble("DistancePerRotation", false, 1.0);
       m_simIsConnected = m_simDevice.createBoolean("Connected", false, true);
+    } else {
+      m_counter = new Counter();
+      m_analogTrigger = new AnalogTrigger(m_dutyCycle);
+      m_analogTrigger.setLimitsDutyCycle(0.25, 0.75);
+      m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
+      m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
     }
 
-    m_analogTrigger.setLimitsDutyCycle(0.25, 0.75);
-    m_counter.setUpSource(m_analogTrigger, AnalogTriggerType.kRisingPulse);
-    m_counter.setDownSource(m_analogTrigger, AnalogTriggerType.kFallingPulse);
-
     SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
   }
 
@@ -138,6 +142,10 @@
    */
   public void setDistancePerRotation(double distancePerRotation) {
     m_distancePerRotation = distancePerRotation;
+
+    if (m_simDistancePerRotation != null) {
+      m_simDistancePerRotation.set(distancePerRotation);
+    }
   }
 
   /**
@@ -173,7 +181,9 @@
    * Reset the Encoder distance to zero.
    */
   public void reset() {
-    m_counter.reset();
+    if (m_counter != null) {
+      m_counter.reset();
+    }
     m_positionOffset = m_dutyCycle.getOutput();
   }
 
@@ -209,16 +219,42 @@
 
   @Override
   public void close() {
-    m_counter.close();
-    m_analogTrigger.close();
+    if (m_counter != null) {
+      m_counter.close();
+    }
+    if (m_analogTrigger != null) {
+      m_analogTrigger.close();
+    }
     if (m_ownsDutyCycle) {
       m_dutyCycle.close();
     }
+    if (m_digitalInput != null) {
+      m_digitalInput.close();
+    }
     if (m_simDevice != null) {
       m_simDevice.close();
     }
   }
 
+  /**
+   * Get the FPGA index for the DutyCycleEncoder.
+   *
+   * @return the FPGA index
+   */
+  @SuppressWarnings("AbbreviationAsWordInName")
+  public int getFPGAIndex() {
+    return m_dutyCycle.getFPGAIndex();
+  }
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  public int getSourceChannel() {
+    return m_dutyCycle.getSourceChannel();
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("AbsoluteEncoder");

wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
index cfefb6a..c1327aa 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -48,11 +48,16 @@
    * Obtains the deploy directory of the program, which is the remote location
    * src/main/deploy is deployed to by default. On the roboRIO, this is
    * /home/lvuser/deploy. In simulation, it is where the simulation was launched
-   * from, in the subdirectory "deploy" (`pwd`/deploy).
+   * from, in the subdirectory "src/main/deploy" (`pwd`/src/main/deploy).
    *
    * @return The deploy directory
    */
   public static File getDeployDirectory() {
-    return new File(getOperatingDirectory(), "deploy");
+    if (RobotBase.isReal()) {
+      return new File(getOperatingDirectory(), "deploy");
+    } else {
+      return new File(getOperatingDirectory(), "src" + File.separator + "main"
+          + File.separator + "deploy");
+    }
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
index 0f884b9..f573cb2 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -127,6 +127,9 @@
    * <p>The buttons are returned in a single 16 bit value with one bit representing the state of
    * each button. The appropriate button is returned as a boolean value.
    *
+   * <p>This method returns true if the button is being held down at the time
+   * that this method is being called.
+   *
    * @param button The button number to be read (starting at 1)
    * @return The state of the button.
    */
@@ -138,6 +141,10 @@
    * Whether the button was pressed since the last check. Button indexes begin at
    * 1.
    *
+   * <p>This method returns true if the button went from not pressed to held down
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you press the button.
+   *
    * @param button The button index, beginning at 1.
    * @return Whether the button was pressed since the last check.
    */
@@ -149,6 +156,10 @@
    * Whether the button was released since the last check. Button indexes begin at
    * 1.
    *
+   * <p>This method returns true if the button went from held down to not pressed
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you release the button.
+   *
    * @param button The button index, beginning at 1.
    * @return Whether the button was released since the last check.
    */
@@ -207,6 +218,15 @@
   }
 
   /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  public boolean isConnected() {
+    return m_ds.isJoystickConnected(m_port);
+  }
+
+  /**
    * Get the type of the HID.
    *
    * @return the type of the HID.

wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
index ca8374a..58b96aa 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,7 +22,7 @@
  * <p>This class is intended to be used by sensor (and other I2C device) drivers. It probably should
  * not be used directly.
  */
-@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+@SuppressWarnings("PMD.GodClass")
 public class I2C implements AutoCloseable {
   public enum Port {
     kOnboard(0), kMXP(1);

wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
index bd21d24..8fb734b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,7 +16,6 @@
 /**
  * Base for sensors to be used with interrupts.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public abstract class InterruptableSensorBase implements AutoCloseable {
   @SuppressWarnings("JavadocMethod")
   public enum WaitResult {

wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
index f1baefb..66079a8 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -43,6 +43,10 @@
   public void startCompetition() {
     robotInit();
 
+    if (isSimulation()) {
+      simulationInit();
+    }
+
     // Tell the DS that the robot is ready to be enabled
     HAL.observeUserProgramStarting();
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
index b9a7a6a..daa5bdb 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -42,7 +42,6 @@
  *   - teleopPeriodic()
  *   - testPeriodic()
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public abstract class IterativeRobotBase extends RobotBase {
   protected double m_period;
 
@@ -90,6 +89,18 @@
   }
 
   /**
+   * Robot-wide simulation initialization code should go here.
+   *
+   * <p>Users should override this method for default Robot-wide simulation
+   * related initialization which will be called when the robot is first
+   * started. It will be called exactly one time after RobotInit is called
+   * only when the robot is in simulation.
+   */
+  public void simulationInit() {
+    System.out.println("Default simulationInit() method... Override me!");
+  }
+
+  /**
    * Initialization code for disabled mode should go here.
    *
    * <p>Users should override this method for initialization code which will be called each time the
@@ -144,6 +155,20 @@
     }
   }
 
+  private boolean m_spFirstRun = true;
+
+  /**
+   * Periodic simulation code should go here.
+   *
+   * <p>This function is called in a simulated robot after user code executes.
+   */
+  public void simulationPeriodic() {
+    if (m_spFirstRun) {
+      System.out.println("Default simulationPeriodic() method... Override me!");
+      m_spFirstRun = false;
+    }
+  }
+
   private boolean m_dpFirstRun = true;
 
   /**
@@ -193,6 +218,7 @@
     }
   }
 
+  @SuppressWarnings("PMD.CyclomaticComplexity")
   protected void loopFunc() {
     m_watchdog.reset();
 
@@ -264,6 +290,12 @@
     m_watchdog.addEpoch("LiveWindow.updateValues()");
     Shuffleboard.update();
     m_watchdog.addEpoch("Shuffleboard.update()");
+
+    if (isSimulation()) {
+      simulationPeriodic();
+      m_watchdog.addEpoch("simulationPeriodic()");
+    }
+
     m_watchdog.disable();
 
     // Warn on loop time overruns

wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
index 60a60bb..a83a346 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,11 +18,11 @@
  * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
  */
 public class Joystick extends GenericHID {
-  static final byte kDefaultXChannel = 0;
-  static final byte kDefaultYChannel = 1;
-  static final byte kDefaultZChannel = 2;
-  static final byte kDefaultTwistChannel = 2;
-  static final byte kDefaultThrottleChannel = 3;
+  public static final byte kDefaultXChannel = 0;
+  public static final byte kDefaultYChannel = 1;
+  public static final byte kDefaultZChannel = 2;
+  public static final byte kDefaultTwistChannel = 2;
+  public static final byte kDefaultThrottleChannel = 3;
 
   /**
    * Represents an analog axis on a joystick.

wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
deleted file mode 100644
index 94bcb31..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
+++ /dev/null
@@ -1,171 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.Arrays;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpiutil.CircularBuffer;
-
-/**
- * This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
- * Static factory methods are provided to create commonly used types of filters.
- *
- * <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
- * a2*y[n-2] + ... + aQ*y[n-Q])
- *
- * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
- * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
- * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
- * front of the feedback term! This is a common convention in signal processing.
- *
- * <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
- * undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
- * noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
- * impact of these high frequency components.  Likewise, a "high pass" filter gets rid of
- * slow-moving signal components, letting you detect large changes more easily.
- *
- * <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
- * the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
- * wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
- * strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
- * PID controller out of this class!
- *
- * <p>For more on filters, we highly recommend the following articles:<br>
- * https://en.wikipedia.org/wiki/Linear_filter<br>
- * https://en.wikipedia.org/wiki/Iir_filter<br>
- * https://en.wikipedia.org/wiki/Fir_filter<br>
- *
- * <p>Note 1: calculate() should be called by the user on a known, regular period. You can use a
- * Notifier for this or do it "inline" with code in a periodic function.
- *
- * <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
- * that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
- * then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
- * to make sure calculate() gets called at the desired, constant frequency!
- */
-public class LinearFilter {
-  private final CircularBuffer m_inputs;
-  private final CircularBuffer m_outputs;
-  private final double[] m_inputGains;
-  private final double[] m_outputGains;
-
-  private static int instances;
-
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param ffGains The "feed forward" or FIR gains.
-   * @param fbGains The "feed back" or IIR gains.
-   */
-  public LinearFilter(double[] ffGains, double[] fbGains) {
-    m_inputs = new CircularBuffer(ffGains.length);
-    m_outputs = new CircularBuffer(fbGains.length);
-    m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
-    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
-
-    instances++;
-    HAL.report(tResourceType.kResourceType_LinearFilter, instances);
-  }
-
-  /**
-   * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
-   * gain = e^(-dt / T), T is the time constant in seconds.
-   *
-   * <p>This filter is stable for time constants greater than zero.
-   *
-   * @param timeConstant The discrete-time time constant in seconds.
-   * @param period       The period in seconds between samples taken by the user.
-   */
-  public static LinearFilter singlePoleIIR(double timeConstant,
-                                           double period) {
-    double gain = Math.exp(-period / timeConstant);
-    double[] ffGains = {1.0 - gain};
-    double[] fbGains = {-gain};
-
-    return new LinearFilter(ffGains, fbGains);
-  }
-
-  /**
-   * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
-   * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
-   *
-   * <p>This filter is stable for time constants greater than zero.
-   *
-   * @param timeConstant The discrete-time time constant in seconds.
-   * @param period       The period in seconds between samples taken by the user.
-   */
-  public static LinearFilter highPass(double timeConstant,
-                                      double period) {
-    double gain = Math.exp(-period / timeConstant);
-    double[] ffGains = {gain, -gain};
-    double[] fbGains = {-gain};
-
-    return new LinearFilter(ffGains, fbGains);
-  }
-
-  /**
-   * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
-   * x[0]).
-   *
-   * <p>This filter is always stable.
-   *
-   * @param taps The number of samples to average over. Higher = smoother but slower.
-   * @throws IllegalArgumentException if number of taps is less than 1.
-   */
-  public static LinearFilter movingAverage(int taps) {
-    if (taps <= 0) {
-      throw new IllegalArgumentException("Number of taps was not at least 1");
-    }
-
-    double[] ffGains = new double[taps];
-    for (int i = 0; i < ffGains.length; i++) {
-      ffGains[i] = 1.0 / taps;
-    }
-
-    double[] fbGains = new double[0];
-
-    return new LinearFilter(ffGains, fbGains);
-  }
-
-  /**
-   * Reset the filter state.
-   */
-  public void reset() {
-    m_inputs.clear();
-    m_outputs.clear();
-  }
-
-  /**
-   * Calculates the next value of the filter.
-   *
-   * @param input Current input value.
-   *
-   * @return The filtered value at this step
-   */
-  public double calculate(double input) {
-    double retVal = 0.0;
-
-    // Rotate the inputs
-    m_inputs.addFirst(input);
-
-    // Calculate the new value
-    for (int i = 0; i < m_inputGains.length; i++) {
-      retVal += m_inputs.get(i) * m_inputGains[i];
-    }
-    for (int i = 0; i < m_outputGains.length; i++) {
-      retVal -= m_outputs.get(i) * m_outputGains[i];
-    }
-
-    // Rotate the outputs
-    m_outputs.addFirst(retVal);
-
-    return retVal;
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
deleted file mode 100644
index a4a3a95..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
+++ /dev/null
@@ -1,86 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
-
-import edu.wpi.first.wpiutil.CircularBuffer;
-
-/**
- * A class that implements a moving-window median filter.  Useful for reducing measurement noise,
- * especially with processes that generate occasional, extreme outliers (such as values from
- * vision processing, LIDAR, or ultrasonic sensors).
- */
-public class MedianFilter {
-  private final CircularBuffer m_valueBuffer;
-  private final List<Double> m_orderedValues;
-  private final int m_size;
-
-  /**
-   * Creates a new MedianFilter.
-   *
-   * @param size The number of samples in the moving window.
-   */
-  public MedianFilter(int size) {
-    // Circular buffer of values currently in the window, ordered by time
-    m_valueBuffer = new CircularBuffer(size);
-    // List of values currently in the window, ordered by value
-    m_orderedValues = new ArrayList<>(size);
-    // Size of rolling window
-    m_size = size;
-  }
-
-  /**
-   * Calculates the moving-window median for the next value of the input stream.
-   *
-   * @param next The next input value.
-   * @return The median of the moving window, updated to include the next value.
-   */
-  public double calculate(double next) {
-    // Find insertion point for next value
-    int index = Collections.binarySearch(m_orderedValues, next);
-
-    // Deal with binarySearch behavior for element not found
-    if (index < 0) {
-      index = Math.abs(index + 1);
-    }
-
-    // Place value at proper insertion point
-    m_orderedValues.add(index, next);
-
-    int curSize = m_orderedValues.size();
-
-    // If buffer is at max size, pop element off of end of circular buffer
-    // and remove from ordered list
-    if (curSize > m_size) {
-      m_orderedValues.remove(m_valueBuffer.removeLast());
-      curSize = curSize - 1;
-    }
-
-    // Add next value to circular buffer
-    m_valueBuffer.addFirst(next);
-
-    if (curSize % 2 == 1) {
-      // If size is odd, return middle element of sorted list
-      return m_orderedValues.get(curSize / 2);
-    } else {
-      // If size is even, return average of middle elements
-      return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
-    }
-  }
-
-  /**
-   * Resets the filter, clearing the window of all elements.
-   */
-  public void reset() {
-    m_orderedValues.clear();
-    m_valueBuffer.clear();
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
index 806bfa9..546da5b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -207,6 +207,12 @@
    * function will block until the handler call is complete.
    */
   public void stop() {
-    NotifierJNI.cancelNotifierAlarm(m_notifier.get());
+    m_processLock.lock();
+    try {
+      m_periodic = false;
+      NotifierJNI.cancelNotifierAlarm(m_notifier.get());
+    } finally {
+      m_processLock.unlock();
+    }
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
index e30be1b..ef46028 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,6 +19,7 @@
  */
 public class PowerDistributionPanel implements Sendable, AutoCloseable {
   private final int m_handle;
+  private final int m_module;
 
   /**
    * Constructor.
@@ -28,6 +29,7 @@
   public PowerDistributionPanel(int module) {
     SensorUtil.checkPDPModule(module);
     m_handle = PDPJNI.initializePDP(module);
+    m_module = module;
 
     HAL.report(tResourceType.kResourceType_PDP, module + 1);
     SendableRegistry.addLW(this, "PowerDistributionPanel", module);
@@ -117,6 +119,13 @@
     PDPJNI.clearPDPStickyFaults(m_handle);
   }
 
+  /**
+   * Gets module number (CAN ID).
+   */
+  public int getModule() {
+    return m_module;
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("PowerDistributionPanel");

wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
index b25d907..0edbbf3 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -95,6 +95,17 @@
   }
 
   /**
+   * Puts the given string into the preferences table if it doesn't already exist.
+   *
+   * @param key   The key
+   * @param value The value
+   */
+  public void initString(String key, String value) {
+    NetworkTableEntry entry = m_table.getEntry(key);
+    entry.setDefaultString(value);
+  }
+
+  /**
    * Puts the given int into the preferences table.
    *
    * @param key   the key
@@ -107,6 +118,17 @@
   }
 
   /**
+   * Puts the given int into the preferences table if it doesn't already exist.
+   *
+   * @param key   The key
+   * @param value The value
+   */
+  public void initInt(String key, int value) {
+    NetworkTableEntry entry = m_table.getEntry(key);
+    entry.setDefaultDouble(value);
+  }
+
+  /**
    * Puts the given double into the preferences table.
    *
    * @param key   the key
@@ -119,6 +141,17 @@
   }
 
   /**
+   * Puts the given double into the preferences table if it doesn't already exist.
+   *
+   * @param key   The key
+   * @param value The value
+   */
+  public void initDouble(String key, double value) {
+    NetworkTableEntry entry = m_table.getEntry(key);
+    entry.setDefaultDouble(value);
+  }
+
+  /**
    * Puts the given float into the preferences table.
    *
    * @param key   the key
@@ -131,6 +164,17 @@
   }
 
   /**
+   * Puts the given float into the preferences table if it doesn't already exist.
+   *
+   * @param key   The key
+   * @param value The value
+   */
+  public void initFloat(String key, float value) {
+    NetworkTableEntry entry = m_table.getEntry(key);
+    entry.setDefaultDouble(value);
+  }
+
+  /**
    * Puts the given boolean into the preferences table.
    *
    * @param key   the key
@@ -143,6 +187,17 @@
   }
 
   /**
+   * Puts the given boolean into the preferences table if it doesn't already exist.
+   *
+   * @param key   The key
+   * @param value The value
+   */
+  public void initBoolean(String key, boolean value) {
+    NetworkTableEntry entry = m_table.getEntry(key);
+    entry.setDefaultBoolean(value);
+  }
+
+  /**
    * Puts the given long into the preferences table.
    *
    * @param key   the key
@@ -155,6 +210,17 @@
   }
 
   /**
+   * Puts the given long into the preferences table if it doesn't already exist.
+   *
+   * @param key   The key
+   * @param value The value
+   */
+  public void initLong(String key, long value) {
+    NetworkTableEntry entry = m_table.getEntry(key);
+    entry.setDefaultDouble(value);
+  }
+
+  /**
    * Returns whether or not there is a key with the given name.
    *
    * @param key the key

wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
index fde4ef1..f379de4 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -33,6 +33,7 @@
    * This class represents errors in trying to set relay values contradictory to the direction to
    * which the relay is set.
    */
+  @SuppressWarnings("serial")
   public static class InvalidValueException extends RuntimeException {
     /**
      * Create a new exception with the given message.

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
index e2861f4..9c3e45f 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,6 +22,9 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.math.MathShared;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -78,6 +81,53 @@
     CameraServerSharedStore.setCameraServerShared(shared);
   }
 
+  private static void setupMathShared() {
+    MathSharedStore.setMathShared(new MathShared() {
+      @Override
+      public void reportError(String error, StackTraceElement[] stackTrace) {
+        DriverStation.reportError(error, stackTrace);
+      }
+
+      @Override
+      public void reportUsage(MathUsageId id, int count) {
+        switch (id) {
+          case kKinematics_DifferentialDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_DifferentialDrive);
+            break;
+          case kKinematics_MecanumDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_MecanumDrive);
+            break;
+          case kKinematics_SwerveDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_SwerveDrive);
+            break;
+          case kTrajectory_TrapezoidProfile:
+            HAL.report(tResourceType.kResourceType_TrapezoidProfile, count);
+            break;
+          case kFilter_Linear:
+            HAL.report(tResourceType.kResourceType_LinearFilter, count);
+            break;
+          case kOdometry_DifferentialDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_DifferentialDrive);
+            break;
+          case kOdometry_SwerveDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_SwerveDrive);
+            break;
+          case kOdometry_MecanumDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_MecanumDrive);
+            break;
+          default:
+            break;
+        }
+      }
+    });
+  }
+
   protected final DriverStation m_ds;
 
   /**
@@ -90,9 +140,10 @@
    * to put this code into it's own task that loads on boot so ensure that it runs.
    */
   protected RobotBase() {
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
+    final NetworkTableInstance inst = NetworkTableInstance.getDefault();
     m_threadId = Thread.currentThread().getId();
     setupCameraServerShared();
+    setupMathShared();
     inst.setNetworkIdentity("Robot");
     if (isReal()) {
       inst.startServer("/home/lvuser/networktables.ini");
@@ -161,6 +212,16 @@
   }
 
   /**
+   * Determine if the robot is current in Autonomous mode and enabled as determined by
+   * the field controls.
+   *
+   * @return True if the robot is currently operating autonomously while enabled.
+   */
+  public boolean isAutonomousEnabled() {
+    return m_ds.isAutonomousEnabled();
+  }
+
+  /**
    * Determine if the robot is currently in Test mode as determined by the driver
    * station.
    *
@@ -181,6 +242,16 @@
   }
 
   /**
+   * Determine if the robot is current in Operator Control mode and enabled as determined by
+   * the field controls.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while enabled.
+   */
+  public boolean isOperatorControlEnabled() {
+    return m_ds.isOperatorControlEnabled();
+  }
+
+  /**
    * Indicates if new data is available from the driver station.
    *
    * @return Has new data arrived over the network since the last time this function was called?
@@ -348,6 +419,8 @@
       runRobot(robotSupplier);
     }
 
-    System.exit(1);
+    HAL.shutdown();
+
+    System.exit(0);
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
index 104fb26..f79d74a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,7 +25,7 @@
  *             or {@link edu.wpi.first.wpilibj.drive.MecanumDrive} classes instead.
  */
 @Deprecated
-@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+@SuppressWarnings("PMD.GodClass")
 public class RobotDrive extends MotorSafety implements AutoCloseable {
   /**
    * The location of a motor on the robot for the purpose of driving.

wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
index aca2cbe..aa3c679 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,7 +19,7 @@
 /**
  * Represents a SPI bus port.
  */
-@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.TooManyMethods"})
+@SuppressWarnings("PMD.CyclomaticComplexity")
 public class SPI implements AutoCloseable {
   public enum Port {
     kOnboardCS0(0), kOnboardCS1(1), kOnboardCS2(2), kOnboardCS3(3), kMXP(4);
@@ -162,9 +162,9 @@
   }
 
   /**
-   * Write data to the slave device. Blocks until there is space in the output FIFO.
+   * Write data to the peripheral device. Blocks until there is space in the output FIFO.
    *
-   * <p>If not running in output only mode, also saves the data received on the MISO input during
+   * <p>If not running in output only mode, also saves the data received on the CIPO input during
    * the transfer into the receive FIFO.
    */
   public int write(byte[] dataToSend, int size) {
@@ -175,9 +175,9 @@
   }
 
   /**
-   * Write data to the slave device. Blocks until there is space in the output FIFO.
+   * Write data to the peripheral device. Blocks until there is space in the output FIFO.
    *
-   * <p>If not running in output only mode, also saves the data received on the MISO input during
+   * <p>If not running in output only mode, also saves the data received on the CIPO input during
    * the transfer into the receive FIFO.
    *
    * @param dataToSend The buffer containing the data to send.

wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
index 4f2b8b5..22aa7bd 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -84,7 +84,7 @@
         .append(kPCMModules)
         .append(", Requested: ")
         .append(moduleNumber);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -101,7 +101,7 @@
         .append(kDigitalChannels)
         .append(", Requested: ")
         .append(channel);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -118,7 +118,7 @@
         .append(kRelayChannels)
         .append(", Requested: ")
         .append(channel);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -135,7 +135,7 @@
         .append(kPwmChannels)
         .append(", Requested: ")
         .append(channel);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -152,7 +152,7 @@
         .append(kAnalogInputChannels)
         .append(", Requested: ")
         .append(channel);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -169,7 +169,7 @@
         .append(kAnalogOutputChannels)
         .append(", Requested: ")
         .append(channel);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -185,7 +185,7 @@
         .append(kSolenoidChannels)
         .append(", Requested: ")
         .append(channel);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -202,7 +202,7 @@
         .append(kPDPChannels)
         .append(", Requested: ")
         .append(channel);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 
@@ -218,7 +218,7 @@
         .append(kPDPModules)
         .append(", Requested: ")
         .append(module);
-      throw new IndexOutOfBoundsException(buf.toString());
+      throw new IllegalArgumentException(buf.toString());
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
index 86bd941..0f509c7 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,7 +16,6 @@
 /**
  * Driver for the serial ports (USB, MXP, Onboard) on the roboRIO.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class SerialPort implements AutoCloseable {
   private int m_portHandle;
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
index 2ba468b..fe4dda7 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,9 +16,9 @@
  * {@link edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead.
  */
 public class SlewRateLimiter {
-  private final Timer m_timer = new Timer();
   private final double m_rateLimit;
   private double m_prevVal;
+  private double m_prevTime;
 
   /**
    * Creates a new SlewRateLimiter with the given rate limit and initial value.
@@ -27,9 +27,9 @@
    * @param initialValue The initial value of the input.
    */
   public SlewRateLimiter(double rateLimit, double initialValue) {
-    m_prevVal = initialValue;
     m_rateLimit = rateLimit;
-    m_timer.start();
+    m_prevVal = initialValue;
+    m_prevTime = Timer.getFPGATimestamp();
   }
 
   /**
@@ -48,10 +48,12 @@
    * @return The filtered value, which will not change faster than the slew rate.
    */
   public double calculate(double input) {
+    double currentTime = Timer.getFPGATimestamp();
+    double elapsedTime = currentTime - m_prevTime;
     m_prevVal += MathUtil.clamp(input - m_prevVal,
-                                -m_rateLimit * m_timer.get(),
-                                m_rateLimit * m_timer.get());
-    m_timer.reset();
+                                -m_rateLimit * elapsedTime,
+                                m_rateLimit * elapsedTime);
+    m_prevTime = currentTime;
     return m_prevVal;
   }
 
@@ -61,7 +63,7 @@
    * @param value The value to reset to.
    */
   public void reset(double value) {
-    m_timer.reset();
     m_prevVal = value;
+    m_prevTime = Timer.getFPGATimestamp();
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
index 163415c..6245c2b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -78,6 +78,16 @@
   }
 
   /**
+   * Toggle the value of the solenoid.
+   *
+   * <p>If the solenoid is set to on, it'll be turned off. If the solenoid is set to off, it'll be
+   * turned on.
+   */
+  public void toggle() {
+    set(!get());
+  }
+
+  /**
    * Check if solenoid is blacklisted. If a solenoid is shorted, it is added to the blacklist and
    * disabled until power cycle, or until faults are cleared.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
index 7dd3f76..81b5470 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 package edu.wpi.first.wpilibj;
 
+import java.util.Arrays;
+
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -28,13 +30,23 @@
                               SpeedController... speedControllers) {
     m_speedControllers = new SpeedController[speedControllers.length + 1];
     m_speedControllers[0] = speedController;
-    SendableRegistry.addChild(this, speedController);
     for (int i = 0; i < speedControllers.length; i++) {
       m_speedControllers[i + 1] = speedControllers[i];
-      SendableRegistry.addChild(this, speedControllers[i]);
+    }
+    init();
+  }
+
+  public SpeedControllerGroup(SpeedController[] speedControllers) {
+    m_speedControllers = Arrays.copyOf(speedControllers, speedControllers.length);
+    init();
+  }
+
+  private void init() {
+    for (SpeedController controller : m_speedControllers) {
+      SendableRegistry.addChild(this, controller);
     }
     instances++;
-    SendableRegistry.addLW(this, "tSpeedControllerGroup", instances);
+    SendableRegistry.addLW(this, "SpeedControllerGroup", instances);
   }
 
   @Override

wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
index b6b6b25..99bce3d 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 package edu.wpi.first.wpilibj;
 
+import java.util.PriorityQueue;
+
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -20,14 +22,48 @@
  * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
  */
 public class TimedRobot extends IterativeRobotBase {
+  @SuppressWarnings("MemberName")
+  class Callback implements Comparable<Callback> {
+    public Runnable func;
+    public double period;
+    public double expirationTime;
+
+    /**
+     * Construct a callback container.
+     *
+     * @param func             The callback to run.
+     * @param startTimeSeconds The common starting point for all callback
+     *                         scheduling in seconds.
+     * @param periodSeconds    The period at which to run the callback in
+     *                         seconds.
+     * @param offsetSeconds    The offset from the common starting time in
+     *                         seconds.
+     */
+    Callback(Runnable func, double startTimeSeconds, double periodSeconds, double offsetSeconds) {
+      this.func = func;
+      this.period = periodSeconds;
+      this.expirationTime = startTimeSeconds + offsetSeconds
+        + Math.floor((Timer.getFPGATimestamp() - startTimeSeconds)
+            / this.period) * this.period + this.period;
+    }
+
+    @Override
+    public int compareTo(Callback rhs) {
+      // Elements with sooner expiration times are sorted as lesser. The head of
+      // Java's PriorityQueue is the least element.
+      return Double.compare(expirationTime, rhs.expirationTime);
+    }
+  }
+
   public static final double kDefaultPeriod = 0.02;
 
   // The C pointer to the notifier object. We don't use it directly, it is
   // just passed to the JNI bindings.
   private final int m_notifier = NotifierJNI.initializeNotifier();
 
-  // The absolute expiration time
-  private double m_expirationTime;
+  private double m_startTime;
+
+  private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
 
   /**
    * Constructor for TimedRobot.
@@ -43,6 +79,8 @@
    */
   protected TimedRobot(double period) {
     super(period);
+    m_startTime = Timer.getFPGATimestamp();
+    addPeriodic(this::loopFunc, period);
     NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
 
     HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed);
@@ -63,23 +101,41 @@
   public void startCompetition() {
     robotInit();
 
+    if (isSimulation()) {
+      simulationInit();
+    }
+
     // Tell the DS that the robot is ready to be enabled
     HAL.observeUserProgramStarting();
 
-    m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period;
-    updateAlarm();
-
     // Loop forever, calling the appropriate mode-dependent function
     while (true) {
+      // We don't have to check there's an element in the queue first because
+      // there's always at least one (the constructor adds one). It's reenqueued
+      // at the end of the loop.
+      var callback = m_callbacks.poll();
+
+      NotifierJNI.updateNotifierAlarm(m_notifier, (long) (callback.expirationTime * 1e6));
+
       long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
       if (curTime == 0) {
         break;
       }
 
-      m_expirationTime += m_period;
-      updateAlarm();
+      callback.func.run();
 
-      loopFunc();
+      callback.expirationTime += callback.period;
+      m_callbacks.add(callback);
+
+      // Process all other callbacks that are ready to run
+      while ((long) (m_callbacks.peek().expirationTime * 1e6) <= curTime) {
+        callback = m_callbacks.poll();
+
+        callback.func.run();
+
+        callback.expirationTime += callback.period;
+        m_callbacks.add(callback);
+      }
     }
   }
 
@@ -99,10 +155,31 @@
   }
 
   /**
-   * Update the alarm hardware to reflect the next alarm.
+   * Add a callback to run at a specific period.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback      The callback to run.
+   * @param periodSeconds The period at which to run the callback in seconds.
    */
-  @SuppressWarnings("UnsafeFinalization")
-  private void updateAlarm() {
-    NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6));
+  public void addPeriodic(Runnable callback, double periodSeconds) {
+    m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, 0.0));
+  }
+
+  /**
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
+   * synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback      The callback to run.
+   * @param periodSeconds The period at which to run the callback in seconds.
+   * @param offsetSeconds The offset from the common starting time in seconds.
+   *                      This is useful for scheduling a callback in a
+   *                      different timeslot relative to TimedRobot.
+   */
+  public void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
+    m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, offsetSeconds));
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
index e1e312f..5d71e72 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,7 +24,7 @@
    * but does send an approximate match time. The value will count down the time remaining in the
    * current period (auto or teleop). Warning: This is not an official time (so it cannot be used to
    * dispute ref calls or guarantee that a function will trigger before the match ends) The
-   * Practice Match function of the DS approximates the behaviour seen on the field.
+   * Practice Match function of the DS approximates the behavior seen on the field.
    *
    * @return Time remaining in current match period (auto or teleop) in seconds
    */
@@ -52,6 +52,11 @@
   private double m_accumulatedTime;
   private boolean m_running;
 
+  /**
+   * Lock for synchronization.
+   */
+  private final Object m_lock = new Object();
+
   @SuppressWarnings("JavadocMethod")
   public Timer() {
     reset();
@@ -68,11 +73,13 @@
    *
    * @return Current time value for this timer in seconds
    */
-  public synchronized double get() {
-    if (m_running) {
-      return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
-    } else {
-      return m_accumulatedTime;
+  public double get() {
+    synchronized (m_lock) {
+      if (m_running) {
+        return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
+      } else {
+        return m_accumulatedTime;
+      }
     }
   }
 
@@ -80,18 +87,25 @@
    * Reset the timer by setting the time to 0. Make the timer startTime the current time so new
    * requests will be relative now
    */
-  public synchronized void reset() {
-    m_accumulatedTime = 0;
-    m_startTime = getMsClock();
+  public void reset() {
+    synchronized (m_lock) {
+      m_accumulatedTime = 0;
+      m_startTime = getMsClock();
+    }
   }
 
   /**
    * Start the timer running. Just set the running flag to true indicating that all time requests
-   * should be relative to the system clock.
+   * should be relative to the system clock. Note that this method is a no-op if the timer is
+   * already running.
    */
-  public synchronized void start() {
-    m_startTime = getMsClock();
-    m_running = true;
+  public void start() {
+    synchronized (m_lock) {
+      if (!m_running) {
+        m_startTime = getMsClock();
+        m_running = true;
+      }
+    }
   }
 
   /**
@@ -99,9 +113,23 @@
    * subsequent time requests to be read from the accumulated time rather than looking at the system
    * clock.
    */
-  public synchronized void stop() {
-    m_accumulatedTime = get();
-    m_running = false;
+  public void stop() {
+    synchronized (m_lock) {
+      m_accumulatedTime = get();
+      m_running = false;
+    }
+  }
+
+  /**
+   * Check if the period specified has passed.
+   *
+   * @param seconds The period to check.
+   * @return Whether the period has passed.
+   */
+  public boolean hasElapsed(double seconds) {
+    synchronized (m_lock) {
+      return get() > seconds;
+    }
   }
 
   /**
@@ -110,15 +138,30 @@
    * took to get around to checking.
    *
    * @param period The period to check for (in seconds).
-   * @return If the period has passed.
+   * @return Whether the period has passed.
    */
-  public synchronized boolean hasPeriodPassed(double period) {
-    if (get() > period) {
-      // Advance the start time by the period.
-      // Don't set it to the current time... we want to avoid drift.
-      m_startTime += period * 1000;
-      return true;
+  public boolean hasPeriodPassed(double period) {
+    return advanceIfElapsed(period);
+  }
+
+  /**
+   * Check if the period specified has passed and if it has, advance the start time by that period.
+   * This is useful to decide if it's time to do periodic work without drifting later by the time it
+   * took to get around to checking.
+   *
+   * @param seconds The period to check.
+   * @return Whether the period has passed.
+   */
+  public boolean advanceIfElapsed(double seconds) {
+    synchronized (m_lock) {
+      if (get() > seconds) {
+        // Advance the start time by the period.
+        // Don't set it to the current time... we want to avoid drift.
+        m_startTime += seconds * 1000;
+        return true;
+      } else {
+        return false;
+      }
     }
-    return false;
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
new file mode 100644
index 0000000..3026e90
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+
+/**
+ * A class for keeping track of how much time it takes for different parts of code to execute.
+ * This is done with epochs, that are added by calls to {@link #addEpoch(String)},
+ * and can be printed with a call to {@link #printEpochs()}.
+ *
+ * <p>Epochs are a way to partition the time elapsed so that when overruns occur, one can
+ * determine which parts of an operation consumed the most time.
+ */
+public class Tracer {
+  private static final long kMinPrintPeriod = 1000000; // microseconds
+
+  private long m_lastEpochsPrintTime; // microseconds
+  private long m_startTime; // microseconds
+
+  @SuppressWarnings("PMD.UseConcurrentHashMap")
+  private final Map<String, Long> m_epochs = new HashMap<>(); // microseconds
+
+  /**
+   * Tracer constructor.
+   */
+  public Tracer() {
+    resetTimer();
+  }
+
+  /**
+   * Clears all epochs.
+   */
+  public void clearEpochs() {
+    m_epochs.clear();
+    resetTimer();
+  }
+
+  /**
+   * Restarts the epoch timer.
+   */
+  public void resetTimer() {
+    m_startTime = RobotController.getFPGATime();
+  }
+
+  /**
+   * Adds time since last epoch to the list printed by printEpochs().
+   *
+   * <p>Epochs are a way to partition the time elapsed so that when overruns occur, one can
+   * determine which parts of an operation consumed the most time.
+   *
+   * <p>This should be called immediately after execution has finished,
+   * with a call to this method or {@link #resetTimer()} before execution.
+   *
+   * @param epochName The name to associate with the epoch.
+   */
+  public void addEpoch(String epochName) {
+    long currentTime = RobotController.getFPGATime();
+    m_epochs.put(epochName, currentTime - m_startTime);
+    m_startTime = currentTime;
+  }
+
+  /**
+   * Prints list of epochs added so far and their times to the DriverStation.
+   */
+  public void printEpochs() {
+    printEpochs(out -> DriverStation.reportWarning(out, false));
+  }
+
+  /**
+   * Prints list of epochs added so far and their times to the entered String consumer.
+   *
+   * <p>This overload can be useful for logging to a file, etc.
+   *
+   * @param output the stream that the output is sent to
+   */
+  public void printEpochs(Consumer<String> output) {
+    long now = RobotController.getFPGATime();
+    if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+      StringBuilder sb = new StringBuilder();
+      m_lastEpochsPrintTime = now;
+      m_epochs.forEach((key, value) -> {
+        sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6));
+      });
+      if (sb.length() > 0) {
+        output.accept(sb.toString());
+      }
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
index db7454a..0e14c96 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -239,8 +239,10 @@
   }
 
   /**
-   * Turn Automatic mode on/off. When in Automatic mode, all sensors will fire in round robin,
-   * waiting a set time between each sensor.
+   * Turn Automatic mode on/off for all sensors.
+   *
+   * <p>When in Automatic mode, all sensors will fire in round robin, waiting a set time between
+   * each sensor.
    *
    * @param enabling Set to true if round robin scheduling should start for all the ultrasonic
    *                 sensors. This scheduling method assures that the sensors are non-interfering
@@ -248,7 +250,7 @@
    *                 is preferred, it can be implemented by pinging the sensors manually and waiting
    *                 for the results to come back.
    */
-  public void setAutomaticMode(boolean enabling) {
+  public static void setAutomaticMode(boolean enabling) {
     if (enabling == m_automaticEnabled) {
       return; // ignore the case of no change
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
index 2d19240..29c1b27 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,7 +17,7 @@
  *
  * <p>Note that the Victor uses the following bounds for PWM values. These values were determined
  * empirically and optimized for the Victor 888. These values should work reasonably well for
- * Victor 884 controllers also but if users experience issues such as asymmetric behaviour around
+ * Victor 884 controllers also but if users experience issues such as asymmetric behavior around
  * the deadband or inability to saturate the controller in either direction, calibration is
  * recommended. The calibration procedure can be found in the Victor 884 User Manual available
  * from VEX Robotics: http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf

wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
index 223e992..aaf5671 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,13 +8,11 @@
 package edu.wpi.first.wpilibj;
 
 import java.io.Closeable;
-import java.util.HashMap;
-import java.util.Map;
 import java.util.PriorityQueue;
-import java.util.concurrent.TimeUnit;
-import java.util.concurrent.locks.Condition;
 import java.util.concurrent.locks.ReentrantLock;
 
+import edu.wpi.first.hal.NotifierJNI;
+
 /**
  * A class that's a wrapper around a watchdog timer.
  *
@@ -23,31 +21,31 @@
  *
  * <p>The watchdog is initialized disabled, so the user needs to call enable() before use.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class Watchdog implements Closeable, Comparable<Watchdog> {
   // Used for timeout print rate-limiting
-  private static final long kMinPrintPeriod = 1000000; // us
+  private static final long kMinPrintPeriod = 1000000; // microseconds
 
-  private long m_startTime; // us
-  private long m_timeout; // us
-  private long m_expirationTime; // us
+  private double m_startTime; // seconds
+  private double m_timeout; // seconds
+  private double m_expirationTime; // seconds
   private final Runnable m_callback;
-  private long m_lastTimeoutPrintTime; // us
-  private long m_lastEpochsPrintTime; // us
+  private double m_lastTimeoutPrintTime; // seconds
 
-  @SuppressWarnings("PMD.UseConcurrentHashMap")
-  private final Map<String, Long> m_epochs = new HashMap<>();
   boolean m_isExpired;
 
   boolean m_suppressTimeoutMessage;
 
-  static {
-    startDaemonThread(Watchdog::schedulerFunc);
-  }
+  private final Tracer m_tracer;
 
   private static final PriorityQueue<Watchdog> m_watchdogs = new PriorityQueue<>();
   private static ReentrantLock m_queueMutex = new ReentrantLock();
-  private static Condition m_schedulerWaiter = m_queueMutex.newCondition();
+  private static int m_notifier;
+
+  static {
+    m_notifier = NotifierJNI.initializeNotifier();
+    NotifierJNI.setNotifierName(m_notifier, "Watchdog");
+    startDaemonThread(Watchdog::schedulerFunc);
+  }
 
   /**
    * Watchdog constructor.
@@ -56,8 +54,9 @@
    * @param callback This function is called when the timeout expires.
    */
   public Watchdog(double timeout, Runnable callback) {
-    m_timeout = (long) (timeout * 1.0e6);
+    m_timeout = timeout;
     m_callback = callback;
+    m_tracer = new Tracer();
   }
 
   @Override
@@ -69,14 +68,28 @@
   public int compareTo(Watchdog rhs) {
     // Elements with sooner expiration times are sorted as lesser. The head of
     // Java's PriorityQueue is the least element.
-    return Long.compare(m_expirationTime, rhs.m_expirationTime);
+    return Double.compare(m_expirationTime, rhs.m_expirationTime);
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (!(obj instanceof Watchdog)) {
+      return false;
+    }
+    Watchdog oth = (Watchdog) obj;
+    return oth.m_expirationTime == m_expirationTime;
+  }
+
+  @Override
+  public int hashCode() {
+    return Double.hashCode(m_expirationTime);
   }
 
   /**
    * Returns the time in seconds since the watchdog was last fed.
    */
   public double getTime() {
-    return (RobotController.getFPGATime() - m_startTime) / 1.0e6;
+    return Timer.getFPGATimestamp() - m_startTime;
   }
 
   /**
@@ -86,18 +99,18 @@
    *                resolution.
    */
   public void setTimeout(double timeout) {
-    m_startTime = RobotController.getFPGATime();
-    m_epochs.clear();
+    m_startTime = Timer.getFPGATimestamp();
+    m_tracer.clearEpochs();
 
     m_queueMutex.lock();
     try {
-      m_timeout = (long) (timeout * 1.0e6);
+      m_timeout = timeout;
       m_isExpired = false;
 
       m_watchdogs.remove(this);
       m_expirationTime = m_startTime + m_timeout;
       m_watchdogs.add(this);
-      m_schedulerWaiter.signalAll();
+      updateAlarm();
     } finally {
       m_queueMutex.unlock();
     }
@@ -109,7 +122,7 @@
   public double getTimeout() {
     m_queueMutex.lock();
     try {
-      return m_timeout / 1.0e6;
+      return m_timeout;
     } finally {
       m_queueMutex.unlock();
     }
@@ -130,26 +143,20 @@
   /**
    * Adds time since last epoch to the list printed by printEpochs().
    *
-   * <p>Epochs are a way to partition the time elapsed so that when overruns occur, one can
-   * determine which parts of an operation consumed the most time.
+   * @see Tracer#addEpoch(String)
    *
    * @param epochName The name to associate with the epoch.
    */
   public void addEpoch(String epochName) {
-    long currentTime = RobotController.getFPGATime();
-    m_epochs.put(epochName, currentTime - m_startTime);
-    m_startTime = currentTime;
+    m_tracer.addEpoch(epochName);
   }
 
   /**
    * Prints list of epochs added so far and their times.
+   * @see Tracer#printEpochs()
    */
   public void printEpochs() {
-    long now = RobotController.getFPGATime();
-    if (now  - m_lastEpochsPrintTime > kMinPrintPeriod) {
-      m_lastEpochsPrintTime = now;
-      m_epochs.forEach((key, value) -> System.out.format("\t%s: %.6fs\n", key, value / 1.0e6));
-    }
+    m_tracer.printEpochs();
   }
 
   /**
@@ -165,8 +172,8 @@
    * Enables the watchdog timer.
    */
   public void enable() {
-    m_startTime = RobotController.getFPGATime();
-    m_epochs.clear();
+    m_startTime = Timer.getFPGATimestamp();
+    m_tracer.clearEpochs();
 
     m_queueMutex.lock();
     try {
@@ -175,7 +182,7 @@
       m_watchdogs.remove(this);
       m_expirationTime = m_startTime + m_timeout;
       m_watchdogs.add(this);
-      m_schedulerWaiter.signalAll();
+      updateAlarm();
     } finally {
       m_queueMutex.unlock();
     }
@@ -188,7 +195,7 @@
     m_queueMutex.lock();
     try {
       m_watchdogs.remove(this);
-      m_schedulerWaiter.signalAll();
+      updateAlarm();
     } finally {
       m_queueMutex.unlock();
     }
@@ -205,6 +212,15 @@
     m_suppressTimeoutMessage = suppress;
   }
 
+  private static void updateAlarm() {
+    if (m_watchdogs.size() == 0) {
+      NotifierJNI.cancelNotifierAlarm(m_notifier);
+    } else {
+      NotifierJNI.updateNotifierAlarm(m_notifier,
+          (long) (m_watchdogs.peek().m_expirationTime * 1e6));
+    }
+  }
+
   private static Thread startDaemonThread(Runnable target) {
     Thread inst = new Thread(target);
     inst.setDaemon(true);
@@ -212,72 +228,47 @@
     return inst;
   }
 
-
   @SuppressWarnings("PMD.AvoidDeeplyNestedIfStmts")
   private static void schedulerFunc() {
-    m_queueMutex.lock();
+    while (!Thread.currentThread().isInterrupted()) {
+      long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
+      if (curTime == 0) {
+        break;
+      }
 
-    try {
-      while (!Thread.currentThread().isInterrupted()) {
-        if (m_watchdogs.size() > 0) {
-          boolean timedOut = !awaitUntil(m_schedulerWaiter, m_watchdogs.peek().m_expirationTime);
-          if (timedOut) {
-            if (m_watchdogs.size() == 0 || m_watchdogs.peek().m_expirationTime
-                > RobotController.getFPGATime()) {
-              continue;
-            }
+      m_queueMutex.lock();
+      try {
+        if (m_watchdogs.size() == 0) {
+          continue;
+        }
 
-            // If the condition variable timed out, that means a Watchdog timeout
-            // has occurred, so call its timeout function.
-            Watchdog watchdog = m_watchdogs.poll();
+        // If the condition variable timed out, that means a Watchdog timeout
+        // has occurred, so call its timeout function.
+        Watchdog watchdog = m_watchdogs.poll();
 
-            long now = RobotController.getFPGATime();
-            if (now  - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
-              watchdog.m_lastTimeoutPrintTime = now;
-              if (!watchdog.m_suppressTimeoutMessage) {
-                System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6);
-              }
-            }
-
-            // Set expiration flag before calling the callback so any
-            // manipulation of the flag in the callback (e.g., calling
-            // Disable()) isn't clobbered.
-            watchdog.m_isExpired = true;
-
-            m_queueMutex.unlock();
-            watchdog.m_callback.run();
-            m_queueMutex.lock();
-          }
-          // Otherwise, a Watchdog removed itself from the queue (it notifies
-          // the scheduler of this) or a spurious wakeup occurred, so just
-          // rewait with the soonest watchdog timeout.
-        } else {
-          while (m_watchdogs.size() == 0) {
-            m_schedulerWaiter.awaitUninterruptibly();
+        double now = curTime * 1e-6;
+        if (now - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
+          watchdog.m_lastTimeoutPrintTime = now;
+          if (!watchdog.m_suppressTimeoutMessage) {
+            DriverStation.reportWarning(
+                String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout),
+                false);
           }
         }
+
+        // Set expiration flag before calling the callback so any
+        // manipulation of the flag in the callback (e.g., calling
+        // Disable()) isn't clobbered.
+        watchdog.m_isExpired = true;
+
+        m_queueMutex.unlock();
+        watchdog.m_callback.run();
+        m_queueMutex.lock();
+
+        updateAlarm();
+      } finally {
+        m_queueMutex.unlock();
       }
-    } finally {
-      m_queueMutex.unlock();
     }
   }
-
-  /**
-   * Wrapper emulating functionality of C++'s std::condition_variable::wait_until().
-   *
-   * @param cond The condition variable on which to wait.
-   * @param time The time at which to stop waiting.
-   * @return False if the deadline has elapsed upon return, else true.
-   */
-  private static boolean awaitUntil(Condition cond, long time) {
-    long delta = time - RobotController.getFPGATime();
-    try {
-      return cond.await(delta, TimeUnit.MICROSECONDS);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-      ex.printStackTrace();
-    }
-
-    return true;
-  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
index fd6161a..6248a76 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -42,6 +42,25 @@
   }
 
   /**
+   * Represents an axis on an XboxController.
+   */
+  public enum Axis {
+    kLeftX(0),
+    kRightX(4),
+    kLeftY(1),
+    kRightY(5),
+    kLeftTrigger(2),
+    kRightTrigger(3);
+
+    @SuppressWarnings({"MemberName", "PMD.SingularField"})
+    public final int value;
+
+    Axis(int value) {
+      this.value = value;
+    }
+  }
+
+  /**
    * Construct an instance of a joystick. The joystick index is the USB port on the drivers
    * station.
    *
@@ -62,9 +81,9 @@
   @Override
   public double getX(Hand hand) {
     if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(0);
+      return getRawAxis(Axis.kLeftX.value);
     } else {
-      return getRawAxis(4);
+      return getRawAxis(Axis.kRightX.value);
     }
   }
 
@@ -77,9 +96,9 @@
   @Override
   public double getY(Hand hand) {
     if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(1);
+      return getRawAxis(Axis.kLeftY.value);
     } else {
-      return getRawAxis(5);
+      return getRawAxis(Axis.kRightY.value);
     }
   }
 
@@ -91,9 +110,9 @@
    */
   public double getTriggerAxis(Hand hand) {
     if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(2);
+      return getRawAxis(Axis.kLeftTrigger.value);
     } else {
-      return getRawAxis(3);
+      return getRawAxis(Axis.kRightTrigger.value);
     }
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
deleted file mode 100644
index 33f9784..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
+++ /dev/null
@@ -1,144 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-/**
- * A helper class that computes feedforward outputs for a simple arm (modeled as a motor
- * acting against the force of gravity on a beam suspended at an angle).
- */
-@SuppressWarnings("MemberName")
-public class ArmFeedforward {
-  public final double ks;
-  public final double kcos;
-  public final double kv;
-  public final double ka;
-
-  /**
-   * Creates a new ArmFeedforward with the specified gains.  Units of the gain values
-   * will dictate units of the computed feedforward.
-   *
-   * @param ks   The static gain.
-   * @param kcos The gravity gain.
-   * @param kv   The velocity gain.
-   * @param ka   The acceleration gain.
-   */
-  public ArmFeedforward(double ks, double kcos, double kv, double ka) {
-    this.ks = ks;
-    this.kcos = kcos;
-    this.kv = kv;
-    this.ka = ka;
-  }
-
-  /**
-   * Creates a new ArmFeedforward with the specified gains.  Acceleration gain is
-   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
-   *
-   * @param ks   The static gain.
-   * @param kcos The gravity gain.
-   * @param kv   The velocity gain.
-   */
-  public ArmFeedforward(double ks, double kcos, double kv) {
-    this(ks, kcos, kv, 0);
-  }
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param positionRadians       The position (angle) setpoint.
-   * @param velocityRadPerSec     The velocity setpoint.
-   * @param accelRadPerSecSquared The acceleration setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double positionRadians, double velocityRadPerSec,
-                          double accelRadPerSecSquared) {
-    return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians)
-        + kv * velocityRadPerSec
-        + ka * accelRadPerSecSquared;
-  }
-
-  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
-   * be zero).
-   *
-   * @param positionRadians The position (angle) setpoint.
-   * @param velocity        The velocity setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double positionRadians, double velocity) {
-    return calculate(positionRadians, velocity, 0);
-  }
-
-  // Rearranging the main equation from the calculate() method yields the
-  // formulas for the methods below:
-
-  /**
-   * Calculates the maximum achievable velocity given a maximum voltage supply,
-   * a position, and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
-   * @param acceleration The acceleration of the arm.
-   * @return The maximum possible velocity at the given acceleration and angle.
-   */
-  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
-  }
-
-  /**
-   * Calculates the minimum achievable velocity given a maximum voltage supply,
-   * a position, and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
-   * @param acceleration The acceleration of the arm.
-   * @return The minimum possible velocity at the given acceleration and angle.
-   */
-  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
-    // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
-  }
-
-  /**
-   * Calculates the maximum achievable acceleration given a maximum voltage
-   * supply, a position, 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
-   * @param velocity The velocity of the arm.
-   * @return The maximum possible acceleration at the given velocity.
-   */
-  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
-    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka;
-  }
-
-  /**
-   * Calculates the minimum achievable acceleration given a maximum voltage
-   * supply, a position, 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the arm.
-   * @param angle The angle of the arm.
-   * @param velocity The velocity of the arm.
-   * @return The minimum possible acceleration at the given velocity.
-   */
-  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
-    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
new file mode 100644
index 0000000..0896ac6
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+public final class ControllerUtil {
+  /**
+   * Returns modulus of error where error is the difference between the reference
+   * and a measurement.
+   *
+   * @param reference Reference input of a controller.
+   * @param measurement The current measurement.
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public static double getModulusError(double reference, double measurement, double minimumInput,
+      double maximumInput) {
+    double error = reference - measurement;
+    double modulus = maximumInput - minimumInput;
+
+    // Wrap error above maximum input
+    int numMax = (int) ((error + maximumInput) / modulus);
+    error -= numMax * modulus;
+
+    // Wrap error below minimum input
+    int numMin = (int) ((error + minimumInput) / modulus);
+    error -= numMin * modulus;
+
+    return error;
+  }
+
+  private ControllerUtil() {
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
deleted file mode 100644
index 86a4f7e..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
+++ /dev/null
@@ -1,135 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-/**
- * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
- * acting against the force of gravity).
- */
-@SuppressWarnings("MemberName")
-public class ElevatorFeedforward {
-  public final double ks;
-  public final double kg;
-  public final double kv;
-  public final double ka;
-
-  /**
-   * Creates a new ElevatorFeedforward with the specified gains.  Units of the gain values
-   * will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
-   */
-  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
-    this.ks = ks;
-    this.kg = kg;
-    this.kv = kv;
-    this.ka = ka;
-  }
-
-  /**
-   * Creates a new ElevatorFeedforward with the specified gains.  Acceleration gain is
-   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
-   */
-  public ElevatorFeedforward(double ks, double kg, double kv) {
-    this(ks, kg, kv, 0);
-  }
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param velocity     The velocity setpoint.
-   * @param acceleration The acceleration setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity, double acceleration) {
-    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
-  }
-
-  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
-   * be zero).
-   *
-   * @param velocity The velocity setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity) {
-    return calculate(velocity, 0);
-  }
-
-  // Rearranging the main equation from the calculate() method yields the
-  // formulas for the methods below:
-
-  /**
-   * Calculates the maximum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param acceleration The acceleration of the elevator.
-   * @return The maximum possible velocity at the given acceleration.
-   */
-  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - ks - kg - acceleration * ka) / kv;
-  }
-
-  /**
-   * Calculates the minimum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param acceleration The acceleration of the elevator.
-   * @return The minimum possible velocity at the given acceleration.
-   */
-  public double minAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
-  }
-
-  /**
-   * Calculates the maximum 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param velocity The velocity of the elevator.
-   * @return The maximum possible acceleration at the given velocity.
-   */
-  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
-    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
-  }
-
-  /**
-   * 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
-   * @param velocity The velocity of the elevator.
-   * @return The minimum possible acceleration at the given velocity.
-   */
-  public double minAchievableAcceleration(double maxVoltage, double velocity) {
-    return maxAchievableAcceleration(-maxVoltage, velocity);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
new file mode 100644
index 0000000..4a98232
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+/**
+ * This holonomic drive controller can be used to follow trajectories using a
+ * holonomic drive train (i.e. swerve or mecanum). Holonomic trajectory following
+ * is a much simpler problem to solve compared to skid-steer style drivetrains because
+ * it is possible to individually control forward, sideways, and angular velocity.
+ *
+ * <p>The holonomic drive controller takes in one PID controller for each direction, forward
+ * and sideways, and one profiled PID controller for the angular direction. Because the
+ * heading dynamics are decoupled from translations, users can specify a custom heading
+ * that the drivetrain should point toward. This heading reference is profiled for smoothness.
+ */
+@SuppressWarnings("MemberName")
+public class HolonomicDriveController {
+  private Pose2d m_poseError = new Pose2d();
+  private Pose2d m_poseTolerance = new Pose2d();
+  private boolean m_enabled = true;
+
+  private final PIDController m_xController;
+  private final PIDController m_yController;
+  private final ProfiledPIDController m_thetaController;
+
+  /**
+   * Constructs a holonomic drive controller.
+   *
+   * @param xController     A PID Controller to respond to error in the field-relative x direction.
+   * @param yController     A PID Controller to respond to error in the field-relative y direction.
+   * @param thetaController A profiled PID controller to respond to error in angle.
+   */
+  @SuppressWarnings("ParameterName")
+  public HolonomicDriveController(PIDController xController,
+                                  PIDController yController,
+                                  ProfiledPIDController thetaController) {
+    m_xController = xController;
+    m_yController = yController;
+    m_thetaController = thetaController;
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   *
+   * @return True if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    final var eTranslate = m_poseError.getTranslation();
+    final var eRotate = m_poseError.getRotation();
+    final var tolTranslate = m_poseTolerance.getTranslation();
+    final var tolRotate = m_poseTolerance.getRotation();
+    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+  }
+
+  /**
+   * Sets the pose error which is considered tolerance for use with atReference().
+   *
+   * @param tolerance The pose error which is tolerable.
+   */
+  public void setTolerance(Pose2d tolerance) {
+    m_poseTolerance = tolerance;
+  }
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * @param currentPose             The current pose.
+   * @param poseRef                 The desired pose.
+   * @param linearVelocityRefMeters The linear velocity reference.
+   * @param angleRef                The angular reference.
+   * @return The next output of the holonomic drive controller.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public ChassisSpeeds calculate(Pose2d currentPose,
+                                 Pose2d poseRef,
+                                 double linearVelocityRefMeters,
+                                 Rotation2d angleRef) {
+    // Calculate feedforward velocities (field-relative).
+    double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
+    double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
+    double thetaFF = m_thetaController.calculate(currentPose.getRotation().getRadians(),
+        angleRef.getRadians());
+
+    m_poseError = poseRef.relativeTo(currentPose);
+
+    if (!m_enabled) {
+      return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
+    }
+
+    // Calculate feedback velocities (based on position error).
+    double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
+    double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
+
+    // Return next output.
+    return ChassisSpeeds.fromFieldRelativeSpeeds(xFF + xFeedback,
+        yFF + yFeedback, thetaFF, currentPose.getRotation());
+  }
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired trajectory state.
+   * @param angleRef     The desired end-angle.
+   * @return The next output of the holonomic drive controller.
+   */
+  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState,
+                                 Rotation2d angleRef) {
+    return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond,
+        angleRef);
+  }
+
+  /**
+   * Enables and disables the controller for troubleshooting problems. When calculate()
+   * is called on a disabled controller, only feedforward values are returned.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  public void setEnabled(boolean enabled) {
+    m_enabled = enabled;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
index eea0644..526e91e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
@@ -40,15 +40,10 @@
 
   private double m_minimumIntegral = -1.0;
 
-  // Maximum input - limit setpoint to this
   private double m_maximumInput;
 
-  // Minimum input - limit setpoint to this
   private double m_minimumInput;
 
-  // Input range - difference between maximum and minimum
-  private double m_inputRange;
-
   // Do the endpoints wrap around? eg. Absolute encoder
   private boolean m_continuous;
 
@@ -62,7 +57,7 @@
   // The sum of the errors for use in the integral calc
   private double m_totalError;
 
-  // The percentage or absolute error that is considered at setpoint.
+  // The error that is considered at setpoint.
   private double m_positionTolerance = 0.05;
   private double m_velocityTolerance = Double.POSITIVE_INFINITY;
 
@@ -196,11 +191,7 @@
    * @param setpoint The desired setpoint.
    */
   public void setSetpoint(double setpoint) {
-    if (m_maximumInput > m_minimumInput) {
-      m_setpoint = MathUtil.clamp(setpoint, m_minimumInput, m_maximumInput);
-    } else {
-      m_setpoint = setpoint;
-    }
+    m_setpoint = setpoint;
   }
 
   /**
@@ -213,8 +204,7 @@
   }
 
   /**
-   * Returns true if the error is within the percentage of the total input range, determined by
-   * SetTolerance. This asssumes that the maximum and minimum input were set using SetInput.
+   * Returns true if the error is within the tolerance of the setpoint.
    *
    * <p>This will return false until at least one input value has been computed.
    *
@@ -237,7 +227,8 @@
    */
   public void enableContinuousInput(double minimumInput, double maximumInput) {
     m_continuous = true;
-    setInputRange(minimumInput, maximumInput);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -248,6 +239,13 @@
   }
 
   /**
+   * Returns true if continuous input is enabled.
+   */
+  public boolean isContinuousInputEnabled() {
+    return m_continuous;
+  }
+
+  /**
    * Sets the minimum and maximum values for the integrator.
    *
    * <p>When the cap is reached, the integrator value is added to the controller
@@ -287,7 +285,7 @@
    * @return The error.
    */
   public double getPositionError() {
-    return getContinuousError(m_positionError);
+    return m_positionError;
   }
 
   /**
@@ -316,7 +314,14 @@
    */
   public double calculate(double measurement) {
     m_prevError = m_positionError;
-    m_positionError = getContinuousError(m_setpoint - measurement);
+
+    if (m_continuous) {
+      m_positionError = ControllerUtil.getModulusError(m_setpoint, measurement, m_minimumInput,
+          m_maximumInput);
+    } else {
+      m_positionError = m_setpoint - measurement;
+    }
+
     m_velocityError = (m_positionError - m_prevError) / m_period;
 
     if (m_Ki != 0) {
@@ -343,42 +348,4 @@
     builder.addDoubleProperty("d", this::getD, this::setD);
     builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
   }
-
-  /**
-   * Wraps error around for continuous inputs. The original error is returned if continuous mode is
-   * disabled.
-   *
-   * @param error The current error of the PID controller.
-   * @return Error for continuous inputs.
-   */
-  protected double getContinuousError(double error) {
-    if (m_continuous && m_inputRange > 0) {
-      error %= m_inputRange;
-      if (Math.abs(error) > m_inputRange / 2) {
-        if (error > 0) {
-          return error - m_inputRange;
-        } else {
-          return error + m_inputRange;
-        }
-      }
-    }
-    return error;
-  }
-
-  /**
-   * Sets the minimum and maximum values expected from the input.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  private void setInputRange(double minimumInput, double maximumInput) {
-    m_minimumInput = minimumInput;
-    m_maximumInput = maximumInput;
-    m_inputRange = maximumInput - minimumInput;
-
-    // Clamp setpoint to new input
-    if (m_maximumInput > m_minimumInput) {
-      m_setpoint = MathUtil.clamp(m_setpoint, m_minimumInput, m_maximumInput);
-    }
-  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
index 7550aa5..3955c5b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
@@ -18,11 +18,12 @@
  * profile. Users should call reset() when they first start running the controller
  * to avoid unwanted behavior.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class ProfiledPIDController implements Sendable {
   private static int instances;
 
   private PIDController m_controller;
+  private double m_minimumInput;
+  private double m_maximumInput;
   private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
   private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
   private TrapezoidProfile.Constraints m_constraints;
@@ -216,6 +217,8 @@
    */
   public void enableContinuousInput(double minimumInput, double maximumInput) {
     m_controller.enableContinuousInput(minimumInput, maximumInput);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -279,6 +282,21 @@
    * @param measurement The current measurement of the process variable.
    */
   public double calculate(double measurement) {
+    if (m_controller.isContinuousInputEnabled()) {
+      // Get error which is smallest distance between goal and measurement
+      double goalMinDistance = ControllerUtil.getModulusError(m_goal.position, measurement,
+          m_minimumInput, m_maximumInput);
+      double setpointMinDistance = ControllerUtil.getModulusError(m_setpoint.position, measurement,
+          m_minimumInput, m_maximumInput);
+
+      // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal
+      // may be outside the input range after this operation, but that's OK because the controller
+      // will still go there and report an error of zero. In other words, the setpoint only needs to
+      // be offset from the measurement by the input range modulus; they don't need to be equal.
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
+    }
+
     var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
     m_setpoint = profile.calculate(getPeriod());
     return m_controller.calculate(measurement, m_setpoint.position);

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
index 3ff9ac5..b597352 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -48,6 +48,7 @@
 
   private Pose2d m_poseError = new Pose2d();
   private Pose2d m_poseTolerance = new Pose2d();
+  private boolean m_enabled = true;
 
   /**
    * Construct a Ramsete unicycle controller.
@@ -65,7 +66,7 @@
 
   /**
    * Construct a Ramsete unicycle controller. The default arguments for
-   * b and zeta of 2.0 and 0.7 have been well-tested to produce desireable
+   * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
    * results.
    */
   public RamseteController() {
@@ -103,19 +104,23 @@
    *
    * @param currentPose                        The current pose.
    * @param poseRef                            The desired pose.
-   * @param linearVelocityRefMeters            The desired linear velocity in meters.
-   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in meters.
+   * @param linearVelocityRefMeters            The desired linear velocity in meters per second.
+   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
    */
   @SuppressWarnings("LocalVariableName")
   public ChassisSpeeds calculate(Pose2d currentPose,
                                  Pose2d poseRef,
                                  double linearVelocityRefMeters,
                                  double angularVelocityRefRadiansPerSecond) {
+    if (!m_enabled) {
+      return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
+    }
+
     m_poseError = poseRef.relativeTo(currentPose);
 
     // Aliases for equation readability
-    final double eX = m_poseError.getTranslation().getX();
-    final double eY = m_poseError.getTranslation().getY();
+    final double eX = m_poseError.getX();
+    final double eY = m_poseError.getY();
     final double eTheta = m_poseError.getRotation().getRadians();
     final double vRef = linearVelocityRefMeters;
     final double omegaRef = angularVelocityRefRadiansPerSecond;
@@ -144,6 +149,15 @@
   }
 
   /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  public void setEnabled(boolean enabled) {
+    m_enabled = enabled;
+  }
+
+  /**
    * Returns sin(x) / x.
    *
    * @param x Value of which to take sinc(x).

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
deleted file mode 100644
index 9d2c57e..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
+++ /dev/null
@@ -1,130 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-/**
- * A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
- */
-@SuppressWarnings("MemberName")
-public class SimpleMotorFeedforward {
-  public final double ks;
-  public final double kv;
-  public final double ka;
-
-  /**
-   * Creates a new SimpleMotorFeedforward with the specified gains.  Units of the gain values
-   * will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
-   */
-  public SimpleMotorFeedforward(double ks, double kv, double ka) {
-    this.ks = ks;
-    this.kv = kv;
-    this.ka = ka;
-  }
-
-  /**
-   * Creates a new SimpleMotorFeedforward with the specified gains.  Acceleration gain is
-   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kv The velocity gain.
-   */
-  public SimpleMotorFeedforward(double ks, double kv) {
-    this(ks, kv, 0);
-  }
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param velocity     The velocity setpoint.
-   * @param acceleration The acceleration setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity, double acceleration) {
-    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
-  }
-
-  // Rearranging the main equation from the calculate() method yields the
-  // formulas for the methods below:
-
-  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
-   * be zero).
-   *
-   * @param velocity The velocity setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity) {
-    return calculate(velocity, 0);
-  }
-
-  /**
-   * Calculates the maximum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param acceleration The acceleration of the motor.
-   * @return The maximum possible velocity at the given acceleration.
-   */
-  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - ks - acceleration * ka) / kv;
-  }
-
-  /**
-   * Calculates the minimum achievable velocity given a maximum voltage supply
-   * and an acceleration.  Useful for ensuring that velocity and
-   * acceleration constraints for a trapezoidal profile are simultaneously
-   * achievable - enter the acceleration constraint, and this will give you
-   * a simultaneously-achievable velocity constraint.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param acceleration The acceleration of the motor.
-   * @return The minimum possible velocity at the given acceleration.
-   */
-  public double minAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + ks - acceleration * ka) / kv;
-  }
-
-  /**
-   * Calculates the maximum 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param velocity The velocity of the motor.
-   * @return The maximum possible acceleration at the given velocity.
-   */
-  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
-    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
-  }
-
-  /**
-   * Calculates the maximum 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.
-   *
-   * @param maxVoltage The maximum voltage that can be supplied to the motor.
-   * @param velocity The velocity of the motor.
-   * @return The minimum possible acceleration at the given velocity.
-   */
-  public double minAchievableAcceleration(double maxVoltage, double velocity) {
-    return maxAchievableAcceleration(-maxVoltage, velocity);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
index cbfd3c0..88a6ff9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -393,7 +393,7 @@
   }
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multipled by -1.
+   * Gets if the power sent to the right side of the drivetrain is multiplied by -1.
    *
    * @return true if the right side is inverted
    */
@@ -402,9 +402,9 @@
   }
 
   /**
-   * Sets if the power sent to the right side of the drivetrain should be multipled by -1.
+   * Sets if the power sent to the right side of the drivetrain should be multiplied by -1.
    *
-   * @param rightSideInverted true if right side power should be multipled by -1
+   * @param rightSideInverted true if right side power should be multiplied by -1
    */
   public void setRightSideInverted(boolean rightSideInverted) {
     m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
index 702bdf5..d709004 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -215,7 +215,7 @@
   }
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multipled by -1.
+   * Gets if the power sent to the right side of the drivetrain is multiplied by -1.
    *
    * @return true if the right side is inverted
    */
@@ -224,9 +224,9 @@
   }
 
   /**
-   * Sets if the power sent to the right side of the drivetrain should be multipled by -1.
+   * Sets if the power sent to the right side of the drivetrain should be multiplied by -1.
    *
-   * @param rightSideInverted true if right side power should be multipled by -1
+   * @param rightSideInverted true if right side power should be multiplied by -1
    */
   public void setRightSideInverted(boolean rightSideInverted) {
     m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
deleted file mode 100644
index 1fa2c8d..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
+++ /dev/null
@@ -1,271 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.io.IOException;
-import java.util.Objects;
-
-import com.fasterxml.jackson.core.JsonGenerator;
-import com.fasterxml.jackson.core.JsonParser;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.DeserializationContext;
-import com.fasterxml.jackson.databind.JsonNode;
-import com.fasterxml.jackson.databind.SerializerProvider;
-import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
-import com.fasterxml.jackson.databind.annotation.JsonSerialize;
-import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
-import com.fasterxml.jackson.databind.ser.std.StdSerializer;
-
-/**
- * Represents a 2d pose containing translational and rotational elements.
- */
-@JsonSerialize(using = Pose2d.PoseSerializer.class)
-@JsonDeserialize(using = Pose2d.PoseDeserializer.class)
-public class Pose2d {
-  private final Translation2d m_translation;
-  private final Rotation2d m_rotation;
-
-  /**
-   * Constructs a pose at the origin facing toward the positive X axis.
-   * (Translation2d{0, 0} and Rotation{0})
-   */
-  public Pose2d() {
-    m_translation = new Translation2d();
-    m_rotation = new Rotation2d();
-  }
-
-  /**
-   * Constructs a pose with the specified translation and rotation.
-   *
-   * @param translation The translational component of the pose.
-   * @param rotation    The rotational component of the pose.
-   */
-  public Pose2d(Translation2d translation, Rotation2d rotation) {
-    m_translation = translation;
-    m_rotation = rotation;
-  }
-
-  /**
-   * Convenience constructors that takes in x and y values directly instead of
-   * having to construct a Translation2d.
-   *
-   * @param x        The x component of the translational component of the pose.
-   * @param y        The y component of the translational component of the pose.
-   * @param rotation The rotational component of the pose.
-   */
-  @SuppressWarnings("ParameterName")
-  public Pose2d(double x, double y, Rotation2d rotation) {
-    m_translation = new Translation2d(x, y);
-    m_rotation = rotation;
-  }
-
-  /**
-   * Transforms the pose by the given transformation and returns the new
-   * transformed pose.
-   *
-   * <p>The matrix multiplication is as follows
-   * [x_new]    [cos, -sin, 0][transform.x]
-   * [y_new] += [sin,  cos, 0][transform.y]
-   * [t_new]    [0,    0,   1][transform.t]
-   *
-   * @param other The transform to transform the pose by.
-   * @return The transformed pose.
-   */
-  public Pose2d plus(Transform2d other) {
-    return transformBy(other);
-  }
-
-  /**
-   * Returns the Transform2d that maps the one pose to another.
-   *
-   * @param other The initial pose of the transformation.
-   * @return The transform that maps the other pose to the current pose.
-   */
-  public Transform2d minus(Pose2d other) {
-    final var pose = this.relativeTo(other);
-    return new Transform2d(pose.getTranslation(), pose.getRotation());
-  }
-
-  /**
-   * Returns the translation component of the transformation.
-   *
-   * @return The translational component of the pose.
-   */
-  public Translation2d getTranslation() {
-    return m_translation;
-  }
-
-  /**
-   * Returns the rotational component of the transformation.
-   *
-   * @return The rotational component of the pose.
-   */
-  public Rotation2d getRotation() {
-    return m_rotation;
-  }
-
-  /**
-   * Transforms the pose by the given transformation and returns the new pose.
-   * See + operator for the matrix multiplication performed.
-   *
-   * @param other The transform to transform the pose by.
-   * @return The transformed pose.
-   */
-  public Pose2d transformBy(Transform2d other) {
-    return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
-        m_rotation.plus(other.getRotation()));
-  }
-
-  /**
-   * Returns the other pose relative to the current pose.
-   *
-   * <p>This function can often be used for trajectory tracking or pose
-   * stabilization algorithms to get the error between the reference and the
-   * current pose.
-   *
-   * @param other The pose that is the origin of the new coordinate frame that
-   *              the current pose will be converted into.
-   * @return The current pose relative to the new origin pose.
-   */
-  public Pose2d relativeTo(Pose2d other) {
-    var transform = new Transform2d(other, this);
-    return new Pose2d(transform.getTranslation(), transform.getRotation());
-  }
-
-  /**
-   * Obtain a new Pose2d from a (constant curvature) velocity.
-   *
-   * <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
-   * Controls Engineering in the FIRST Robotics Competition</a>
-   * section on nonlinear pose estimation for derivation.
-   *
-   * <p>The twist is a change in pose in the robot's coordinate frame since the
-   * previous pose update. When the user runs exp() on the previous known
-   * field-relative pose with the argument being the twist, the user will
-   * receive the new field-relative pose.
-   *
-   * <p>"Exp" represents the pose exponential, which is solving a differential
-   * equation moving the pose forward in time.
-   *
-   * @param twist The change in pose in the robot's coordinate frame since the
-   *              previous pose update. For example, if a non-holonomic robot moves forward
-   *              0.01 meters and changes angle by 0.5 degrees since the previous pose update,
-   *              the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
-   * @return The new pose of the robot.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public Pose2d exp(Twist2d twist) {
-    double dx = twist.dx;
-    double dy = twist.dy;
-    double dtheta = twist.dtheta;
-
-    double sinTheta = Math.sin(dtheta);
-    double cosTheta = Math.cos(dtheta);
-
-    double s;
-    double c;
-    if (Math.abs(dtheta) < 1E-9) {
-      s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
-      c = 0.5 * dtheta;
-    } else {
-      s = sinTheta / dtheta;
-      c = (1 - cosTheta) / dtheta;
-    }
-    var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
-        new Rotation2d(cosTheta, sinTheta));
-
-    return this.plus(transform);
-  }
-
-  /**
-   * Returns a Twist2d that maps this pose to the end pose. If c is the output
-   * of a.Log(b), then a.Exp(c) would yield b.
-   *
-   * @param end The end pose for the transformation.
-   * @return The twist that maps this to end.
-   */
-  public Twist2d log(Pose2d end) {
-    final var transform = end.relativeTo(this);
-    final var dtheta = transform.getRotation().getRadians();
-    final var halfDtheta = dtheta / 2.0;
-
-    final var cosMinusOne = transform.getRotation().getCos() - 1;
-
-    double halfThetaByTanOfHalfDtheta;
-    if (Math.abs(cosMinusOne) < 1E-9) {
-      halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
-    } else {
-      halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
-    }
-
-    Translation2d translationPart = transform.getTranslation().rotateBy(
-        new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
-    ).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
-
-    return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
-  }
-
-  /**
-   * Checks equality between this Pose2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Pose2d) {
-      return ((Pose2d) obj).m_translation.equals(m_translation)
-          && ((Pose2d) obj).m_rotation.equals(m_rotation);
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_translation, m_rotation);
-  }
-
-  static class PoseSerializer extends StdSerializer<Pose2d> {
-    PoseSerializer() {
-      super(Pose2d.class);
-    }
-
-    @Override
-    public void serialize(
-            Pose2d value, JsonGenerator jgen, SerializerProvider provider)
-            throws IOException, JsonProcessingException {
-
-      jgen.writeStartObject();
-      jgen.writeObjectField("translation", value.m_translation);
-      jgen.writeObjectField("rotation", value.m_rotation);
-      jgen.writeEndObject();
-    }
-  }
-
-  static class PoseDeserializer extends StdDeserializer<Pose2d> {
-    PoseDeserializer() {
-      super(Pose2d.class);
-    }
-
-    @Override
-    public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
-            throws IOException, JsonProcessingException {
-      JsonNode node = jp.getCodec().readTree(jp);
-
-      Translation2d translation =
-              jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
-      Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
-      return new Pose2d(translation, rotation);
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
deleted file mode 100644
index 288fa5c..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
+++ /dev/null
@@ -1,251 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.io.IOException;
-import java.util.Objects;
-
-import com.fasterxml.jackson.core.JsonGenerator;
-import com.fasterxml.jackson.core.JsonParser;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.DeserializationContext;
-import com.fasterxml.jackson.databind.JsonNode;
-import com.fasterxml.jackson.databind.SerializerProvider;
-import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
-import com.fasterxml.jackson.databind.annotation.JsonSerialize;
-import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
-import com.fasterxml.jackson.databind.ser.std.StdSerializer;
-
-/**
- * A rotation in a 2d coordinate frame represented a point on the unit circle
- * (cosine and sine).
- */
-@JsonSerialize(using = Rotation2d.RotationSerializer.class)
-@JsonDeserialize(using = Rotation2d.RotationDeserializer.class)
-public class Rotation2d {
-  private final double m_value;
-  private final double m_cos;
-  private final double m_sin;
-
-  /**
-   * Constructs a Rotation2d with a default angle of 0 degrees.
-   */
-  public Rotation2d() {
-    m_value = 0.0;
-    m_cos = 1.0;
-    m_sin = 0.0;
-  }
-
-  /**
-   * Constructs a Rotation2d with the given radian value.
-   * The x and y don't have to be normalized.
-   *
-   * @param value The value of the angle in radians.
-   */
-  public Rotation2d(double value) {
-    m_value = value;
-    m_cos = Math.cos(value);
-    m_sin = Math.sin(value);
-  }
-
-  /**
-   * Constructs a Rotation2d with the given x and y (cosine and sine)
-   * components.
-   *
-   * @param x The x component or cosine of the rotation.
-   * @param y The y component or sine of the rotation.
-   */
-  @SuppressWarnings("ParameterName")
-  public Rotation2d(double x, double y) {
-    double magnitude = Math.hypot(x, y);
-    if (magnitude > 1e-6) {
-      m_sin = y / magnitude;
-      m_cos = x / magnitude;
-    } else {
-      m_sin = 0.0;
-      m_cos = 1.0;
-    }
-    m_value = Math.atan2(m_sin, m_cos);
-  }
-
-  /**
-   * Constructs and returns a Rotation2d with the given degree value.
-   *
-   * @param degrees The value of the angle in degrees.
-   * @return The rotation object with the desired angle value.
-   */
-  public static Rotation2d fromDegrees(double degrees) {
-    return new Rotation2d(Math.toRadians(degrees));
-  }
-
-  /**
-   * Adds two rotations together, with the result being bounded between -pi and
-   * pi.
-   *
-   * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to add.
-   * @return The sum of the two rotations.
-   */
-  public Rotation2d plus(Rotation2d other) {
-    return rotateBy(other);
-  }
-
-  /**
-   * Subtracts the new rotation from the current rotation and returns the new
-   * rotation.
-   *
-   * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to subtract.
-   * @return The difference between the two rotations.
-   */
-  public Rotation2d minus(Rotation2d other) {
-    return rotateBy(other.unaryMinus());
-  }
-
-  /**
-   * Takes the inverse of the current rotation. This is simply the negative of
-   * the current angular value.
-   *
-   * @return The inverse of the current rotation.
-   */
-  public Rotation2d unaryMinus() {
-    return new Rotation2d(-m_value);
-  }
-
-  /**
-   * Multiplies the current rotation by a scalar.
-   *
-   * @param scalar The scalar.
-   * @return The new scaled Rotation2d.
-   */
-  public Rotation2d times(double scalar) {
-    return new Rotation2d(m_value * scalar);
-  }
-
-  /**
-   * Adds the new rotation to the current rotation using a rotation matrix.
-   *
-   * <p>The matrix multiplication is as follows:
-   * [cos_new]   [other.cos, -other.sin][cos]
-   * [sin_new] = [other.sin,  other.cos][sin]
-   * value_new = atan2(cos_new, sin_new)
-   *
-   * @param other The rotation to rotate by.
-   * @return The new rotated Rotation2d.
-   */
-  public Rotation2d rotateBy(Rotation2d other) {
-    return new Rotation2d(
-        m_cos * other.m_cos - m_sin * other.m_sin,
-        m_cos * other.m_sin + m_sin * other.m_cos
-    );
-  }
-
-  /*
-   * Returns the radian value of the rotation.
-   *
-   * @return The radian value of the rotation.
-   */
-  public double getRadians() {
-    return m_value;
-  }
-
-  /**
-   * Returns the degree value of the rotation.
-   *
-   * @return The degree value of the rotation.
-   */
-  public double getDegrees() {
-    return Math.toDegrees(m_value);
-  }
-
-  /**
-   * Returns the cosine of the rotation.
-   *
-   * @return The cosine of the rotation.
-   */
-  public double getCos() {
-    return m_cos;
-  }
-
-  /**
-   * Returns the sine of the rotation.
-   *
-   * @return The sine of the rotation.
-   */
-  public double getSin() {
-    return m_sin;
-  }
-
-  /**
-   * Returns the tangent of the rotation.
-   *
-   * @return The tangent of the rotation.
-   */
-  public double getTan() {
-    return m_sin / m_cos;
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
-  }
-
-  /**
-   * Checks equality between this Rotation2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Rotation2d) {
-      return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_value);
-  }
-
-  static class RotationSerializer extends StdSerializer<Rotation2d> {
-    RotationSerializer() {
-      super(Rotation2d.class);
-    }
-
-    @Override
-    public void serialize(
-            Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
-            throws IOException, JsonProcessingException {
-
-      jgen.writeStartObject();
-      jgen.writeNumberField("radians", value.m_value);
-      jgen.writeEndObject();
-    }
-  }
-
-  static class RotationDeserializer extends StdDeserializer<Rotation2d> {
-    RotationDeserializer() {
-      super(Rotation2d.class);
-    }
-
-    @Override
-    public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
-            throws IOException, JsonProcessingException {
-      JsonNode node = jp.getCodec().readTree(jp);
-      double radians = node.get("radians").numberValue().doubleValue();
-
-      return new Rotation2d(radians);
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
deleted file mode 100644
index 507ebfe..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
+++ /dev/null
@@ -1,106 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.util.Objects;
-
-/**
- * Represents a transformation for a Pose2d.
- */
-public class Transform2d {
-  private final Translation2d m_translation;
-  private final Rotation2d m_rotation;
-
-  /**
-   * Constructs the transform that maps the initial pose to the final pose.
-   *
-   * @param initial The initial pose for the transformation.
-   * @param last    The final pose for the transformation.
-   */
-  public Transform2d(Pose2d initial, Pose2d last) {
-    // We are rotating the difference between the translations
-    // using a clockwise rotation matrix. This transforms the global
-    // delta into a local delta (relative to the initial pose).
-    m_translation = last.getTranslation().minus(initial.getTranslation())
-            .rotateBy(initial.getRotation().unaryMinus());
-
-    m_rotation = last.getRotation().minus(initial.getRotation());
-  }
-
-  /**
-   * Constructs a transform with the given translation and rotation components.
-   *
-   * @param translation Translational component of the transform.
-   * @param rotation    Rotational component of the transform.
-   */
-  public Transform2d(Translation2d translation, Rotation2d rotation) {
-    m_translation = translation;
-    m_rotation = rotation;
-  }
-
-  /**
-   * Constructs the identity transform -- maps an initial pose to itself.
-   */
-  public Transform2d() {
-    m_translation = new Translation2d();
-    m_rotation = new Rotation2d();
-  }
-
-  /**
-   * Scales the transform by the scalar.
-   *
-   * @param scalar The scalar.
-   * @return The scaled Transform2d.
-   */
-  public Transform2d times(double scalar) {
-    return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
-  }
-
-  /**
-   * Returns the translation component of the transformation.
-   *
-   * @return The translational component of the transform.
-   */
-  public Translation2d getTranslation() {
-    return m_translation;
-  }
-
-  /**
-   * Returns the rotational component of the transformation.
-   *
-   * @return Reference to the rotational component of the transform.
-   */
-  public Rotation2d getRotation() {
-    return m_rotation;
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
-  }
-
-  /**
-   * Checks equality between this Transform2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Transform2d) {
-      return ((Transform2d) obj).m_translation.equals(m_translation)
-          && ((Transform2d) obj).m_rotation.equals(m_rotation);
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_translation, m_rotation);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
deleted file mode 100644
index 73b0257..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
+++ /dev/null
@@ -1,239 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.io.IOException;
-import java.util.Objects;
-
-import com.fasterxml.jackson.core.JsonGenerator;
-import com.fasterxml.jackson.core.JsonParser;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.DeserializationContext;
-import com.fasterxml.jackson.databind.JsonNode;
-import com.fasterxml.jackson.databind.SerializerProvider;
-import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
-import com.fasterxml.jackson.databind.annotation.JsonSerialize;
-import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
-import com.fasterxml.jackson.databind.ser.std.StdSerializer;
-
-/**
- * Represents a translation in 2d space.
- * This object can be used to represent a point or a vector.
- *
- * <p>This assumes that you are using conventional mathematical axes.
- * When the robot is placed on the origin, facing toward the X direction,
- * moving forward increases the X, whereas moving to the left increases the Y.
- */
-@JsonSerialize(using = Translation2d.TranslationSerializer.class)
-@JsonDeserialize(using = Translation2d.TranslationDeserializer.class)
-@SuppressWarnings({"ParameterName", "MemberName"})
-public class Translation2d {
-  private final double m_x;
-  private final double m_y;
-
-  /**
-   * Constructs a Translation2d with X and Y components equal to zero.
-   */
-  public Translation2d() {
-    this(0.0, 0.0);
-  }
-
-  /**
-   * Constructs a Translation2d with the X and Y components equal to the
-   * provided values.
-   *
-   * @param x The x component of the translation.
-   * @param y The y component of the translation.
-   */
-  public Translation2d(double x, double y) {
-    m_x = x;
-    m_y = y;
-  }
-
-  /**
-   * Calculates the distance between two translations in 2d space.
-   *
-   * <p>This function uses the pythagorean theorem to calculate the distance.
-   * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
-   *
-   * @param other The translation to compute the distance to.
-   * @return The distance between the two translations.
-   */
-  public double getDistance(Translation2d other) {
-    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
-  }
-
-  /**
-   * Returns the X component of the translation.
-   *
-   * @return The x component of the translation.
-   */
-  public double getX() {
-    return m_x;
-  }
-
-  /**
-   * Returns the Y component of the translation.
-   *
-   * @return The y component of the translation.
-   */
-  public double getY() {
-    return m_y;
-  }
-
-  /**
-   * Returns the norm, or distance from the origin to the translation.
-   *
-   * @return The norm of the translation.
-   */
-  public double getNorm() {
-    return Math.hypot(m_x, m_y);
-  }
-
-  /**
-   * Applies a rotation to the translation in 2d space.
-   *
-   * <p>This multiplies the translation vector by a counterclockwise rotation
-   * matrix of the given angle.
-   * [x_new]   [other.cos, -other.sin][x]
-   * [y_new] = [other.sin,  other.cos][y]
-   *
-   * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
-   * Translation2d of {0, 2}.
-   *
-   * @param other The rotation to rotate the translation by.
-   * @return The new rotated translation.
-   */
-  public Translation2d rotateBy(Rotation2d other) {
-    return new Translation2d(
-            m_x * other.getCos() - m_y * other.getSin(),
-            m_x * other.getSin() + m_y * other.getCos()
-    );
-  }
-
-  /**
-   * Adds two translations in 2d space and returns the sum. This is similar to
-   * vector addition.
-   *
-   * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
-   * Translation2d{3.0, 8.0}
-   *
-   * @param other The translation to add.
-   * @return The sum of the translations.
-   */
-  public Translation2d plus(Translation2d other) {
-    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
-  }
-
-  /**
-   * Subtracts the other translation from the other translation and returns the
-   * difference.
-   *
-   * <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
-   * Translation2d{4.0, 2.0}
-   *
-   * @param other The translation to subtract.
-   * @return The difference between the two translations.
-   */
-  public Translation2d minus(Translation2d other) {
-    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
-  }
-
-  /**
-   * Returns the inverse of the current translation. This is equivalent to
-   * rotating by 180 degrees, flipping the point over both axes, or simply
-   * negating both components of the translation.
-   *
-   * @return The inverse of the current translation.
-   */
-  public Translation2d unaryMinus() {
-    return new Translation2d(-m_x, -m_y);
-  }
-
-  /**
-   * Multiplies the translation by a scalar and returns the new translation.
-   *
-   * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
-   *
-   * @param scalar The scalar to multiply by.
-   * @return The scaled translation.
-   */
-  public Translation2d times(double scalar) {
-    return new Translation2d(m_x * scalar, m_y * scalar);
-  }
-
-  /**
-   * Divides the translation by a scalar and returns the new translation.
-   *
-   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
-   *
-   * @param scalar The scalar to multiply by.
-   * @return The reference to the new mutated object.
-   */
-  public Translation2d div(double scalar) {
-    return new Translation2d(m_x / scalar, m_y / scalar);
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
-  }
-
-  /**
-   * Checks equality between this Translation2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Translation2d) {
-      return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
-          && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_x, m_y);
-  }
-
-  static class TranslationSerializer extends StdSerializer<Translation2d> {
-    TranslationSerializer() {
-      super(Translation2d.class);
-    }
-
-    @Override
-    public void serialize(
-            Translation2d value, JsonGenerator jgen, SerializerProvider provider)
-            throws IOException, JsonProcessingException {
-
-      jgen.writeStartObject();
-      jgen.writeNumberField("x", value.m_x);
-      jgen.writeNumberField("y", value.m_y);
-      jgen.writeEndObject();
-    }
-  }
-
-  static class TranslationDeserializer extends StdDeserializer<Translation2d> {
-    TranslationDeserializer() {
-      super(Translation2d.class);
-    }
-
-    @Override
-    public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
-            throws IOException, JsonProcessingException {
-      JsonNode node = jp.getCodec().readTree(jp);
-      double xval = node.get("x").numberValue().doubleValue();
-      double yval = node.get("y").numberValue().doubleValue();
-
-      return new Translation2d(xval, yval);
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
deleted file mode 100644
index f2def81..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
+++ /dev/null
@@ -1,76 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.util.Objects;
-
-/**
- * A change in distance along arc since the last pose update. We can use ideas
- * from differential calculus to create new Pose2ds from a Twist2d and vise
- * versa.
- *
- * <p>A Twist can be used to represent a difference between two poses.
- */
-@SuppressWarnings("MemberName")
-public class Twist2d {
-  /**
-   * Linear "dx" component.
-   */
-  public double dx;
-
-  /**
-   * Linear "dy" component.
-   */
-  public double dy;
-
-  /**
-   * Angular "dtheta" component (radians).
-   */
-  public double dtheta;
-
-  public Twist2d() {
-  }
-
-  /**
-   * Constructs a Twist2d with the given values.
-   * @param dx Change in x direction relative to robot.
-   * @param dy Change in y direction relative to robot.
-   * @param dtheta Change in angle relative to robot.
-   */
-  public Twist2d(double dx, double dy, double dtheta) {
-    this.dx = dx;
-    this.dy = dy;
-    this.dtheta = dtheta;
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
-  }
-
-  /**
-   * Checks equality between this Twist2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Twist2d) {
-      return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
-          && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
-          && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(dx, dy, dtheta);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
index a21e6d5..3bac20f 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,16 +7,16 @@
 
 package edu.wpi.first.wpilibj.interfaces;
 
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
 /**
  * Interface for yaw rate gyros.
  */
 public interface Gyro extends AutoCloseable {
   /**
-   * Calibrate the gyro by running for a number of samples and computing the center value. Then use
-   * the center value as the Accumulator center value for subsequent measurements. It's important to
-   * make sure that the robot is not moving while the centering calculations are in progress, this
-   * is typically done when the robot is first turned on while it's sitting at rest before the
-   * competition starts.
+   * Calibrate the gyro. It's important to make sure that the robot is not moving while the
+   * calibration is in progress, this is typically done when the robot is first turned on while
+   * it's sitting at rest before the match starts.
    */
   void calibrate();
 
@@ -27,16 +27,14 @@
   void reset();
 
   /**
-   * Return the actual angle in degrees that the robot is currently facing.
+   * Return the heading of the robot in degrees.
    *
-   * <p>The angle is based on the current accumulator value corrected by the oversampling rate, the
-   * gyro type and the A/D calibration values. The angle is continuous, that is it will continue
-   * from 360 to 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in
-   * the gyro output as it sweeps past from 360 to 0 on the second time around.
+   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
+   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past
+   * from 360 to 0 on the second time around.
    *
    * <p>The angle is expected to increase as the gyro turns clockwise when looked
-   * at from the top. It needs to follow NED axis conventions in order to work
-   * properly with dependent control loops.
+   * at from the top. It needs to follow the NED axis convention.
    *
    * <p>This heading is based on integration of the returned rate from the gyro.
    *
@@ -50,10 +48,28 @@
    * <p>The rate is based on the most recent reading of the gyro analog value
    *
    * <p>The rate is expected to be positive as the gyro turns clockwise when looked
-   * at from the top. It needs to follow NED axis conventions in order to work
-   * properly with dependent control loops.
+   * at from the top. It needs to follow the NED axis convention.
    *
    * @return the current rate in degrees per second
    */
   double getRate();
+
+  /**
+   * Return the heading of the robot as a {@link edu.wpi.first.wpilibj.geometry.Rotation2d}.
+   *
+   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
+   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past
+   * from 360 to 0 on the second time around.
+   *
+   * <p>The angle is expected to increase as the gyro turns counterclockwise
+   * when looked at from the top. It needs to follow the NWU axis convention.
+   *
+   * <p>This heading is based on integration of the returned rate from the gyro.
+   *
+   * @return the current heading of the robot as a
+   *         {@link edu.wpi.first.wpilibj.geometry.Rotation2d}.
+   */
+  default Rotation2d getRotation2d() {
+    return Rotation2d.fromDegrees(-getAngle());
+  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
deleted file mode 100644
index aa98a47..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
+++ /dev/null
@@ -1,91 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-/**
- * Represents the speed of a robot chassis. Although this struct contains
- * similar members compared to a Twist2d, they do NOT represent the same thing.
- * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference,
- * this ChassisSpeeds struct represents a velocity w.r.t to the robot frame of
- * reference.
- *
- * <p>A strictly non-holonomic drivetrain, such as a differential drive, should
- * never have a dy component because it can never move sideways. Holonomic
- * drivetrains such as swerve and mecanum will often have all three components.
- */
-@SuppressWarnings("MemberName")
-public class ChassisSpeeds {
-  /**
-   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
-   */
-  public double vxMetersPerSecond;
-
-  /**
-   * Represents sideways velocity w.r.t the robot frame of reference. (Left is +)
-   */
-  public double vyMetersPerSecond;
-
-  /**
-   * Represents the angular velocity of the robot frame. (CCW is +)
-   */
-  public double omegaRadiansPerSecond;
-
-  /**
-   * Constructs a ChassisSpeeds with zeros for dx, dy, and theta.
-   */
-  public ChassisSpeeds() {
-  }
-
-  /**
-   * Constructs a ChassisSpeeds object.
-   *
-   * @param vxMetersPerSecond     Forward velocity.
-   * @param vyMetersPerSecond     Sideways velocity.
-   * @param omegaRadiansPerSecond Angular velocity.
-   */
-  public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond,
-                       double omegaRadiansPerSecond) {
-    this.vxMetersPerSecond = vxMetersPerSecond;
-    this.vyMetersPerSecond = vyMetersPerSecond;
-    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
-  }
-
-  /**
-   * Converts a user provided field-relative set of speeds into a robot-relative
-   * ChassisSpeeds object.
-   *
-   * @param vxMetersPerSecond     The component of speed in the x direction relative to the field.
-   *                              Positive x is away from your alliance wall.
-   * @param vyMetersPerSecond     The component of speed in the y direction relative to the field.
-   *                              Positive y is to your left when standing behind the alliance wall.
-   * @param omegaRadiansPerSecond The angular rate of the robot.
-   * @param robotAngle            The angle of the robot as measured by a gyroscope. The robot's
-   *                              angle is considered to be zero when it is facing directly away
-   *                              from your alliance station wall. Remember that this should
-   *                              be CCW positive.
-   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
-   */
-  public static ChassisSpeeds fromFieldRelativeSpeeds(
-      double vxMetersPerSecond, double vyMetersPerSecond,
-      double omegaRadiansPerSecond, Rotation2d robotAngle) {
-    return new ChassisSpeeds(
-        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
-        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
-        omegaRadiansPerSecond
-    );
-  }
-
-  @Override
-  public String toString() {
-    return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
-        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
deleted file mode 100644
index bb632af..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-
-/**
- * Helper class that converts a chassis velocity (dx and dtheta components) to
- * left and right wheel velocities for a differential drive.
- *
- * <p>Inverse kinematics converts a desired chassis speed into left and right
- * velocity components whereas forward kinematics converts left and right
- * component velocities into a linear and angular chassis speed.
- */
-@SuppressWarnings("MemberName")
-public class DifferentialDriveKinematics {
-  public final double trackWidthMeters;
-
-  /**
-   * Constructs a differential drive kinematics object.
-   *
-   * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is
-   *                         the distance between the left wheels and right wheels.
-   *                         However, the empirical value may be larger than the physical
-   *                         measured value due to scrubbing effects.
-   */
-  public DifferentialDriveKinematics(double trackWidthMeters) {
-    this.trackWidthMeters = trackWidthMeters;
-    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_DifferentialDrive);
-  }
-
-  /**
-   * Returns a chassis speed from left and right component velocities using
-   * forward kinematics.
-   *
-   * @param wheelSpeeds The left and right velocities.
-   * @return The chassis speed.
-   */
-  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
-    return new ChassisSpeeds(
-        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
-        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
-            / trackWidthMeters
-    );
-  }
-
-  /**
-   * Returns left and right component velocities from a chassis speed using
-   * inverse kinematics.
-   *
-   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
-   *                      represent the chassis' speed.
-   * @return The left and right velocities.
-   */
-  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
-    return new DifferentialDriveWheelSpeeds(
-        chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2
-          * chassisSpeeds.omegaRadiansPerSecond,
-        chassisSpeeds.vxMetersPerSecond + trackWidthMeters / 2
-          * chassisSpeeds.omegaRadiansPerSecond
-    );
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
deleted file mode 100644
index 35a4342..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
+++ /dev/null
@@ -1,120 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-
-/**
- * Class for differential drive odometry. Odometry allows you to track the
- * robot's position on the field over the course of a match using readings from
- * 2 encoders and a gyroscope.
- *
- * <p>Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- *
- * <p>It is important that you reset your encoders to zero before using this class.
- * Any subsequent pose resets also require the encoders to be reset to zero.
- */
-public class DifferentialDriveOdometry {
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private double m_prevLeftDistance;
-  private double m_prevRightDistance;
-
-  /**
-   * Constructs a DifferentialDriveOdometry object.
-   *
-   * @param gyroAngle         The angle reported by the gyroscope.
-   * @param initialPoseMeters The starting position of the robot on the field.
-   */
-  public DifferentialDriveOdometry(Rotation2d gyroAngle,
-                                   Pose2d initialPoseMeters) {
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_DifferentialDrive);
-  }
-
-  /**
-   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   */
-  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
-    this(gyroAngle, new Pose2d());
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>You NEED to reset your encoders (to zero) when calling this method.
-   *
-   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
-   * The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param poseMeters The position on the field that your robot is at.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-
-    m_prevLeftDistance = 0.0;
-    m_prevRightDistance = 0.0;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-
-  /**
-   * Updates the robot position on the field using distance measurements from encoders. This
-   * method is more numerically accurate than using velocities to integrate the pose and
-   * is also advantageous for teams that are using lower CPR encoders.
-   *
-   * @param gyroAngle           The angle reported by the gyroscope.
-   * @param leftDistanceMeters  The distance traveled by the left encoder.
-   * @param rightDistanceMeters The distance traveled by the right encoder.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
-                       double rightDistanceMeters) {
-    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
-    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-
-    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var newPose = m_poseMeters.exp(
-        new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    return m_poseMeters;
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
deleted file mode 100644
index ac4a9fa..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-/**
- * Represents the wheel speeds for a differential drive drivetrain.
- */
-@SuppressWarnings("MemberName")
-public class DifferentialDriveWheelSpeeds {
-  /**
-   * Speed of the left side of the robot.
-   */
-  public double leftMetersPerSecond;
-
-  /**
-   * Speed of the right side of the robot.
-   */
-  public double rightMetersPerSecond;
-
-  /**
-   * Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds.
-   */
-  public DifferentialDriveWheelSpeeds() {
-  }
-
-  /**
-   * Constructs a DifferentialDriveWheelSpeeds.
-   *
-   * @param leftMetersPerSecond  The left speed.
-   * @param rightMetersPerSecond The right speed.
-   */
-  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
-    this.leftMetersPerSecond = leftMetersPerSecond;
-    this.rightMetersPerSecond = rightMetersPerSecond;
-  }
-
-  /**
-   * 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.
-   *
-   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
-   */
-  public void normalize(double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
-
-    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
-      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-    }
-  }
-
-  @Override
-  public String toString() {
-    return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
-        leftMetersPerSecond, rightMetersPerSecond);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
deleted file mode 100644
index a5e5b5f..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
+++ /dev/null
@@ -1,173 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual wheel speeds.
- *
- * <p>The inverse kinematics (converting from a desired chassis velocity to
- * individual wheel speeds) uses the relative locations of the wheels with
- * respect to the center of rotation. The center of rotation for inverse
- * kinematics is also variable. This means that you can set your set your center
- * of rotation in a corner of the robot to perform special evasion manuevers.
- *
- * <p>Forward kinematics (converting an array of wheel speeds into the overall
- * chassis motion) is performs the exact opposite of what inverse kinematics
- * does. Since this is an overdetermined system (more equations than variables),
- * we use a least-squares approximation.
- *
- * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
- * multiply by [wheelSpeeds] to get our chassis speeds.
- *
- * <p>Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-public class MecanumDriveKinematics {
-  private SimpleMatrix m_inverseKinematics;
-  private final SimpleMatrix m_forwardKinematics;
-
-  private final Translation2d m_frontLeftWheelMeters;
-  private final Translation2d m_frontRightWheelMeters;
-  private final Translation2d m_rearLeftWheelMeters;
-  private final Translation2d m_rearRightWheelMeters;
-
-  private Translation2d m_prevCoR = new Translation2d();
-
-  /**
-   * Constructs a mecanum drive kinematics object.
-   *
-   * @param frontLeftWheelMeters  The location of the front-left wheel relative to the
-   *                              physical center of the robot.
-   * @param frontRightWheelMeters The location of the front-right wheel relative to
-   *                              the physical center of the robot.
-   * @param rearLeftWheelMeters   The location of the rear-left wheel relative to the
-   *                              physical center of the robot.
-   * @param rearRightWheelMeters  The location of the rear-right wheel relative to the
-   *                              physical center of the robot.
-   */
-  public MecanumDriveKinematics(Translation2d frontLeftWheelMeters,
-                                Translation2d frontRightWheelMeters,
-                                Translation2d rearLeftWheelMeters,
-                                Translation2d rearRightWheelMeters) {
-    m_frontLeftWheelMeters = frontLeftWheelMeters;
-    m_frontRightWheelMeters = frontRightWheelMeters;
-    m_rearLeftWheelMeters = rearLeftWheelMeters;
-    m_rearRightWheelMeters = rearRightWheelMeters;
-
-    m_inverseKinematics = new SimpleMatrix(4, 3);
-
-    setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
-        rearLeftWheelMeters, rearRightWheelMeters);
-    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
-
-    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_MecanumDrive);
-  }
-
-  /**
-   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
-   * method is often used to convert joystick values into wheel speeds.
-   *
-   * <p>This function also supports variable centers of rotation. During normal
-   * operations, the center of rotation is usually the same as the physical
-   * center of the robot; therefore, the argument is defaulted to that use case.
-   * However, if you wish to change the center of rotation for evasive
-   * manuevers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds    The desired chassis speed.
-   * @param centerOfRotationMeters The center of rotation. For example, if you set the
-   *                         center of rotation at one corner of the robot and provide
-   *                         a chassis speed that only has a dtheta 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.
-   */
-  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
-                                               Translation2d centerOfRotationMeters) {
-    // We have a new center of rotation. We need to compute the matrix again.
-    if (!centerOfRotationMeters.equals(m_prevCoR)) {
-      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
-      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
-      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
-      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
-
-      setInverseKinematics(fl, fr, rl, rr);
-      m_prevCoR = centerOfRotationMeters;
-    }
-
-    var chassisSpeedsVector = new SimpleMatrix(3, 1);
-    chassisSpeedsVector.setColumn(0, 0,
-        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
-        chassisSpeeds.omegaRadiansPerSecond);
-
-    var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
-    return new MecanumDriveWheelSpeeds(
-        wheelsMatrix.get(0, 0),
-        wheelsMatrix.get(1, 0),
-        wheelsMatrix.get(2, 0),
-        wheelsMatrix.get(3, 0)
-    );
-  }
-
-  /**
-   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
-   * information.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @return The wheel speeds.
-   */
-  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
-    return toWheelSpeeds(chassisSpeeds, new Translation2d());
-  }
-
-  /**
-   * Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
-   * This method is often used for odometry -- determining the robot's position on the field using
-   * data from the real-world speed of each wheel on the robot.
-   *
-   * @param wheelSpeeds The current mecanum drive wheel speeds.
-   * @return The resulting chassis speed.
-   */
-  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
-    var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
-    wheelSpeedsMatrix.setColumn(0, 0,
-        wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond,
-        wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond
-    );
-    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
-
-    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
-        chassisSpeedsVector.get(2, 0));
-  }
-
-  /**
-   * Construct inverse kinematics matrix from wheel locations.
-   *
-   * @param fl The location of the front-left wheel relative to the physical center of the robot.
-   * @param fr The location of the front-right wheel relative to the physical center of the robot.
-   * @param rl The location of the rear-left wheel relative to the physical center of the robot.
-   * @param rr The location of the rear-right wheel relative to the physical center of the robot.
-   */
-  private void setInverseKinematics(Translation2d fl, Translation2d fr,
-                                    Translation2d rl, Translation2d rr) {
-    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
-    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
-    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
-    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
-    m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
deleted file mode 100644
index f467d20..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
+++ /dev/null
@@ -1,65 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-/**
- * Represents the motor voltages for a mecanum drive drivetrain.
- */
-@SuppressWarnings("MemberName")
-public class MecanumDriveMotorVoltages {
-  /**
-   * Voltage of the front left motor.
-   */
-  public double frontLeftVoltage;
-
-  /**
-   * Voltage of the front right motor.
-   */
-  public double frontRightVoltage;
-
-  /**
-   * Voltage of the rear left motor.
-   */
-  public double rearLeftVoltage;
-
-  /**
-   * Voltage of the rear right motor.
-   */
-  public double rearRightVoltage;
-
-  /**
-   * Constructs a MecanumDriveMotorVoltages with zeros for all member fields.
-   */
-  public MecanumDriveMotorVoltages() {
-  }
-
-  /**
-   * Constructs a MecanumDriveMotorVoltages.
-   *
-   * @param frontLeftVoltage  Voltage of the front left motor.
-   * @param frontRightVoltage Voltage of the front right motor.
-   * @param rearLeftVoltage   Voltage of the rear left motor.
-   * @param rearRightVoltage  Voltage of the rear right motor.
-   */
-  public MecanumDriveMotorVoltages(double frontLeftVoltage,
-                                 double frontRightVoltage,
-                                 double rearLeftVoltage,
-                                 double rearRightVoltage) {
-    this.frontLeftVoltage = frontLeftVoltage;
-    this.frontRightVoltage = frontRightVoltage;
-    this.rearLeftVoltage = rearLeftVoltage;
-    this.rearRightVoltage = rearRightVoltage;
-  }
-
-  @Override
-  public String toString() {
-    return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
-            + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
-        rearLeftVoltage, rearRightVoltage);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
deleted file mode 100644
index b3dd402..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-
-/**
- * Class for mecanum drive odometry. Odometry allows you to track the robot's
- * position on the field over a course of a match using readings from your
- * mecanum wheel encoders.
- *
- * <p>Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- */
-public class MecanumDriveOdometry {
-  private final MecanumDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  /**
-   * Constructs a MecanumDriveOdometry object.
-   *
-   * @param kinematics        The mecanum drive kinematics for your drivetrain.
-   * @param gyroAngle         The angle reported by the gyroscope.
-   * @param initialPoseMeters The starting position of the robot on the field.
-   */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
-                              Pose2d initialPoseMeters) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_MecanumDrive);
-  }
-
-  /**
-   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
-   *
-   * @param kinematics The mecanum drive kinematics for your drivetrain.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
-   * The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param poseMeters The position on the field that your robot is at.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param currentTimeSeconds The current time in seconds.
-   * @param gyroAngle          The angle reported by the gyroscope.
-   * @param wheelSpeeds        The current wheel speeds.
-   * @return The new pose of the robot.
-   */
-  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
-                               MecanumDriveWheelSpeeds wheelSpeeds) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
-    var newPose = m_poseMeters.exp(
-        new Twist2d(chassisState.vxMetersPerSecond * period,
-            chassisState.vyMetersPerSecond * period,
-            angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates the
-   * current time to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle   The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle,
-                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle,
-        wheelSpeeds);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
deleted file mode 100644
index 0f0dd71..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
+++ /dev/null
@@ -1,91 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import java.util.stream.DoubleStream;
-
-@SuppressWarnings("MemberName")
-public class MecanumDriveWheelSpeeds {
-  /**
-   * Speed of the front left wheel.
-   */
-  public double frontLeftMetersPerSecond;
-
-  /**
-   * Speed of the front right wheel.
-   */
-  public double frontRightMetersPerSecond;
-
-  /**
-   * Speed of the rear left wheel.
-   */
-  public double rearLeftMetersPerSecond;
-
-  /**
-   * Speed of the rear right wheel.
-   */
-  public double rearRightMetersPerSecond;
-
-  /**
-   * Constructs a MecanumDriveWheelSpeeds with zeros for all member fields.
-   */
-  public MecanumDriveWheelSpeeds() {
-  }
-
-  /**
-   * Constructs a MecanumDriveWheelSpeeds.
-   *
-   * @param frontLeftMetersPerSecond  Speed of the front left wheel.
-   * @param frontRightMetersPerSecond Speed of the front right wheel.
-   * @param rearLeftMetersPerSecond   Speed of the rear left wheel.
-   * @param rearRightMetersPerSecond  Speed of the rear right wheel.
-   */
-  public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond,
-                                 double frontRightMetersPerSecond,
-                                 double rearLeftMetersPerSecond,
-                                 double rearRightMetersPerSecond) {
-    this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
-    this.frontRightMetersPerSecond = frontRightMetersPerSecond;
-    this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
-    this.rearRightMetersPerSecond = rearRightMetersPerSecond;
-  }
-
-  /**
-   * 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.
-   *
-   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
-   */
-  public void normalize(double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond,
-        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond)
-        .max().getAsDouble();
-
-    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
-      frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-    }
-  }
-
-  @Override
-  public String toString() {
-    return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
-            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
-        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
deleted file mode 100644
index 37f97b8..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
+++ /dev/null
@@ -1,200 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import java.util.Arrays;
-import java.util.Collections;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual module states (speed and angle).
- *
- * <p>The inverse kinematics (converting from a desired chassis velocity to
- * individual module states) uses the relative locations of the modules with
- * respect to the center of rotation. The center of rotation for inverse
- * kinematics is also variable. This means that you can set your set your center
- * of rotation in a corner of the robot to perform special evasion manuevers.
- *
- * <p>Forward kinematics (converting an array of module states into the overall
- * chassis motion) is performs the exact opposite of what inverse kinematics
- * does. Since this is an overdetermined system (more equations than variables),
- * we use a least-squares approximation.
- *
- * <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
- * multiply by [moduleStates] to get our chassis speeds.
- *
- * <p>Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-public class SwerveDriveKinematics {
-  private final SimpleMatrix m_inverseKinematics;
-  private final SimpleMatrix m_forwardKinematics;
-
-  private final int m_numModules;
-  private final Translation2d[] m_modules;
-  private Translation2d m_prevCoR = new Translation2d();
-
-  /**
-   * Constructs a swerve drive kinematics object. This takes in a variable
-   * number of wheel locations as Translation2ds. The order in which you pass in
-   * the wheel locations is the same order that you will recieve the module
-   * states when performing inverse kinematics. It is also expected that you
-   * pass in the module states in the same order when calling the forward
-   * kinematics methods.
-   *
-   * @param wheelsMeters The locations of the wheels relative to the physical center
-   *                     of the robot.
-   */
-  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
-    if (wheelsMeters.length < 2) {
-      throw new IllegalArgumentException("A swerve drive requires at least two modules");
-    }
-    m_numModules = wheelsMeters.length;
-    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
-    m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
-
-    for (int i = 0; i < m_numModules; i++) {
-      m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
-      m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
-    }
-    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
-
-    HAL.report(tResourceType.kResourceType_Kinematics, tInstances.kKinematics_SwerveDrive);
-  }
-
-  /**
-   * Performs inverse kinematics to return the module states from a desired
-   * chassis velocity. This method is often used to convert joystick values into
-   * module speeds and angles.
-   *
-   * <p>This function also supports variable centers of rotation. During normal
-   * operations, the center of rotation is usually the same as the physical
-   * center of the robot; therefore, the argument is defaulted to that use case.
-   * However, if you wish to change the center of rotation for evasive
-   * manuevers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds    The desired chassis speed.
-   * @param centerOfRotationMeters The center of rotation. For example, if you set the
-   *                         center of rotation at one corner of the robot and provide
-   *                         a chassis speed that only has a dtheta 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.
-   */
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
-                                                  Translation2d centerOfRotationMeters) {
-    if (!centerOfRotationMeters.equals(m_prevCoR)) {
-      for (int i = 0; i < m_numModules; i++) {
-        m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
-            -m_modules[i].getY() + centerOfRotationMeters.getY());
-        m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1,
-            +m_modules[i].getX() - centerOfRotationMeters.getX());
-      }
-      m_prevCoR = centerOfRotationMeters;
-    }
-
-    var chassisSpeedsVector = new SimpleMatrix(3, 1);
-    chassisSpeedsVector.setColumn(0, 0,
-        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
-        chassisSpeeds.omegaRadiansPerSecond);
-
-    var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
-    SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
-
-    for (int i = 0; i < m_numModules; i++) {
-      double x = moduleStatesMatrix.get(i * 2, 0);
-      double y = moduleStatesMatrix.get(i * 2 + 1, 0);
-
-      double speed = Math.hypot(x, y);
-      Rotation2d angle = new Rotation2d(x, y);
-
-      moduleStates[i] = new SwerveModuleState(speed, angle);
-    }
-
-    return moduleStates;
-  }
-
-  /**
-   * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
-   * toSwerveModuleStates for more information.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @return An array containing the module states.
-   */
-  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
-    return toSwerveModuleStates(chassisSpeeds, new Translation2d());
-  }
-
-  /**
-   * Performs forward kinematics to return the resulting chassis state from the
-   * given module states. This method is often used for odometry -- determining
-   * the robot's position on the field using data from the real-world speed and
-   * angle of each module on the robot.
-   *
-   * @param wheelStates The state of the modules (as a SwerveModuleState type)
-   *                    as measured from respective encoders and gyros. The order of the swerve
-   *                    module states should be same as passed into the constructor of this class.
-   * @return The resulting chassis speed.
-   */
-  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
-    if (wheelStates.length != m_numModules) {
-      throw new IllegalArgumentException(
-          "Number of modules is not consistent with number of wheel locations provided in "
-              + "constructor"
-      );
-    }
-    var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
-
-    for (int i = 0; i < m_numModules; i++) {
-      var module = wheelStates[i];
-      moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
-      moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
-    }
-
-    var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
-    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
-        chassisSpeedsVector.get(2, 0));
-
-  }
-
-  /**
-   * 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.
-   *
-   * @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(SwerveModuleState[] moduleStates,
-                                          double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
-    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
-      for (SwerveModuleState moduleState : moduleStates) {
-        moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed
-            * attainableMaxSpeedMetersPerSecond;
-      }
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
deleted file mode 100644
index 811c3de..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
+++ /dev/null
@@ -1,136 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-
-/**
- * Class for swerve drive odometry. Odometry allows you to track the robot's
- * position on the field over a course of a match using readings from your
- * swerve drive encoders and swerve azimuth encoders.
- *
- * <p>Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- */
-public class SwerveDriveOdometry {
-  private final SwerveDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  /**
-   * Constructs a SwerveDriveOdometry object.
-   *
-   * @param kinematics  The swerve drive kinematics for your drivetrain.
-   * @param gyroAngle   The angle reported by the gyroscope.
-   * @param initialPose The starting position of the robot on the field.
-   */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
-                             Pose2d initialPose) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPose;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPose.getRotation();
-    HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_SwerveDrive);
-  }
-
-  /**
-   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
-   *
-   * @param kinematics The swerve drive kinematics for your drivetrain.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
-   * The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param pose      The position on the field that your robot is at.
-   * @param gyroAngle The angle reported by the gyroscope.
-   */
-  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
-    m_poseMeters = pose;
-    m_previousAngle = pose.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param currentTimeSeconds The current time in seconds.
-   * @param gyroAngle          The angle reported by the gyroscope.
-   * @param moduleStates       The current state of all swerve modules. Please provide
-   *                           the states in the same order in which you instantiated your
-   *                           SwerveDriveKinematics.
-   * @return The new pose of the robot.
-   */
-  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
-                               SwerveModuleState... moduleStates) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
-    var newPose = m_poseMeters.exp(
-        new Twist2d(chassisState.vxMetersPerSecond * period,
-            chassisState.vyMetersPerSecond * period,
-            angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates the
-   * current time to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the angular
-   * rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle    The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide
-   *                     the states in the same order in which you instantiated your
-   *                     SwerveDriveKinematics.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
deleted file mode 100644
index e3119e5..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
+++ /dev/null
@@ -1,63 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-/**
- * Represents the state of one swerve module.
- */
-@SuppressWarnings("MemberName")
-public class SwerveModuleState implements Comparable<SwerveModuleState> {
-
-  /**
-   * Speed of the wheel of the module.
-   */
-  public double speedMetersPerSecond;
-
-  /**
-   * Angle of the module.
-   */
-  public Rotation2d angle = Rotation2d.fromDegrees(0);
-
-  /**
-   * Constructs a SwerveModuleState with zeros for speed and angle.
-   */
-  public SwerveModuleState() {
-  }
-
-  /**
-   * Constructs a SwerveModuleState.
-   *
-   * @param speedMetersPerSecond The speed of the wheel of the module.
-   * @param angle The angle of the module.
-   */
-  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
-    this.speedMetersPerSecond = speedMetersPerSecond;
-    this.angle = angle;
-  }
-
-  /**
-   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
-   * is higher than the other.
-   *
-   * @param o The other swerve module.
-   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
-   */
-  @Override
-  @SuppressWarnings("ParameterName")
-  public int compareTo(SwerveModuleState o) {
-    return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
-  }
-
-  @Override
-  public String toString() {
-    return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
-        angle);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
index 5a4fb36..6570780 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -33,6 +33,9 @@
   private static boolean liveWindowEnabled;
   private static boolean telemetryEnabled = true;
 
+  private static Runnable enabledListener;
+  private static Runnable disabledListener;
+
   private static Component getOrAdd(Sendable sendable) {
     Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
     if (data == null) {
@@ -46,6 +49,14 @@
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
+  public static synchronized void setEnabledListener(Runnable runnable) {
+    enabledListener = runnable;
+  }
+
+  public static synchronized void setDisabledListener(Runnable runnable) {
+    disabledListener = runnable;
+  }
+
   public static synchronized boolean isEnabled() {
     return liveWindowEnabled;
   }
@@ -65,11 +76,17 @@
       updateValues(); // Force table generation now to make sure everything is defined
       if (enabled) {
         System.out.println("Starting live window mode.");
+        if (enabledListener != null) {
+          enabledListener.run();
+        }
       } else {
         System.out.println("stopping live window mode.");
         SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
           cbdata.builder.stopLiveWindowMode();
         });
+        if (disabledListener != null) {
+          disabledListener.run();
+        }
       }
       enabledEntry.setBoolean(enabled);
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
index 14742e5..fa98a0e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -190,7 +190,7 @@
    */
   kSplitButtonChooser("Split Button Chooser"),
   /**
-   * Displays an {@link edu.wpi.first.wpilibj.Encoder} displaying its speed, total travelled
+   * Displays an {@link edu.wpi.first.wpilibj.Encoder} displaying its speed, total traveled
    * distance, and its distance per tick.
    * <br>Supported types:
    * <ul>
@@ -208,7 +208,10 @@
    * <li>{@link edu.wpi.first.wpilibj.PWMSpeedController}</li>
    * <li>{@link edu.wpi.first.wpilibj.DMC60}</li>
    * <li>{@link edu.wpi.first.wpilibj.Jaguar}</li>
+   * <li>{@link edu.wpi.first.wpilibj.PWMSparkMax}</li>
+   * <li>{@link edu.wpi.first.wpilibj.PWMTalonFX}</li>
    * <li>{@link edu.wpi.first.wpilibj.PWMTalonSRX}</li>
+   * <li>{@link edu.wpi.first.wpilibj.PWMVenom}</li>
    * <li>{@link edu.wpi.first.wpilibj.PWMVictorSPX}</li>
    * <li>{@link edu.wpi.first.wpilibj.SD540}</li>
    * <li>{@link edu.wpi.first.wpilibj.Spark}</li>
@@ -216,6 +219,7 @@
    * <li>{@link edu.wpi.first.wpilibj.Victor}</li>
    * <li>{@link edu.wpi.first.wpilibj.VictorSP}</li>
    * <li>{@link edu.wpi.first.wpilibj.SpeedControllerGroup}</li>
+   * <li>Any custom subclass of {@code SpeedController}</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -283,7 +287,7 @@
    */
   kAccelerometer("Accelerometer"),
   /**
-   * Displays a 3-axis accelerometer with a number bar for each axis' accleration.
+   * Displays a 3-axis accelerometer with a number bar for each axis' acceleration.
    * <br>Supported types:
    * <ul>
    * <li>{@link edu.wpi.first.wpilibj.ADXL345_I2C}</li>

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
index a18e4c8..26649d0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -27,7 +27,6 @@
 /**
  * A helper class for Shuffleboard containers to handle common child operations.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 final class ContainerHelper {
   private final ShuffleboardContainer m_container;
   private final Set<String> m_usedTitles = new HashSet<>();
@@ -44,7 +43,7 @@
 
   ShuffleboardLayout getLayout(String title, String type) {
     if (!m_layouts.containsKey(title)) {
-      ShuffleboardLayout layout = new ShuffleboardLayout(m_container, type, title);
+      ShuffleboardLayout layout = new ShuffleboardLayout(m_container, title, type);
       m_components.add(layout);
       m_layouts.put(title, layout);
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
index 331b7f1..9a89404 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,7 +19,6 @@
 /**
  * Common interface for objects that can contain shuffleboard components.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public interface ShuffleboardContainer extends ShuffleboardValue {
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
index 8f3014a..b6ed776 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,13 +21,12 @@
 /**
  * A layout in a Shuffleboard tab. Layouts can contain widgets and other layouts.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class ShuffleboardLayout extends ShuffleboardComponent<ShuffleboardLayout>
     implements ShuffleboardContainer {
   private final ContainerHelper m_helper = new ContainerHelper(this);
 
-  ShuffleboardLayout(ShuffleboardContainer parent, String name, String type) {
-    super(parent, requireNonNullParam(type, "type", "ShuffleboardLayout"), name);
+  ShuffleboardLayout(ShuffleboardContainer parent, String title, String type) {
+    super(parent, title, requireNonNullParam(type, "type", "ShuffleboardLayout"));
   }
 
   @Override

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
index b46c6d2..5d575df 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,7 +22,6 @@
  * can also be added to layouts with {@link #getLayout(String, String)}; layouts can be nested
  * arbitrarily deep (note that too many levels may make deeper components unusable).
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public final class ShuffleboardTab implements ShuffleboardContainer {
   private final ContainerHelper m_helper = new ContainerHelper(this);
   private final ShuffleboardRoot m_root;

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
index 3dc6622..c929f4a 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -36,7 +36,7 @@
   /**
    * Sets the type of widget used to display the data. If not set, the default widget type will be
    * used. This method should only be used to use a widget that does not come built into
-   * Shuffleboard (i.e. one that comes with a custom or thrid-party plugin). To use a widget that
+   * Shuffleboard (i.e. one that comes with a custom or third-party plugin). To use a widget that
    * is built into Shuffleboard, use {@link #withWidget(WidgetType)} and {@link BuiltInWidgets}.
    *
    * @param widgetType the type of the widget used to display the data

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java
new file mode 100644
index 0000000..950efef
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+@SuppressWarnings("TypeName")
+public class ADXRS450_GyroSim {
+  private final SimDouble m_simAngle;
+  private final SimDouble m_simRate;
+
+  /**
+   * Constructs from an ADXRS450_Gyro object.
+   *
+   * @param gyro ADXRS450_Gyro to simulate
+   */
+  public ADXRS450_GyroSim(ADXRS450_Gyro gyro) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("ADXRS450_Gyro" + "[" + gyro.getPort() + "]");
+    m_simAngle = wrappedSimDevice.getDouble("Angle");
+    m_simRate = wrappedSimDevice.getDouble("Rate");
+  }
+
+  /**
+   * Sets the angle in degrees (clockwise positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setAngle(double angleDegrees) {
+    m_simAngle.set(angleDegrees);
+  }
+
+  /**
+   * Sets the angular rate in degrees per second (clockwise positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setRate(double rateDegreesPerSecond) {
+    m_simRate.set(rateDegreesPerSecond);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java
new file mode 100644
index 0000000..81c9a7b
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AddressableLEDDataJNI;
+import edu.wpi.first.hal.simulation.ConstBufferCallback;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.AddressableLED;
+import java.util.NoSuchElementException;
+
+/**
+ * Class to control a simulated addressable LED.
+ */
+public class AddressableLEDSim {
+  private final int m_index;
+
+  /**
+   * Constructs for the first addressable LED.
+   */
+  public AddressableLEDSim() {
+    m_index = 0;
+  }
+
+  /**
+   * Constructs from an AddressableLED object.
+   *
+   * @param addressableLED AddressableLED to simulate
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public AddressableLEDSim(AddressableLED addressableLED) {
+    // there is only support for a single AddressableLED, so no lookup
+    m_index = 0;
+  }
+
+  private AddressableLEDSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates an AddressableLEDSim for a PWM channel.
+   *
+   * @param pwmChannel PWM channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no AddressableLED is configured for that channel
+   */
+  public static AddressableLEDSim createForChannel(int pwmChannel) {
+    int index = AddressableLEDDataJNI.findForChannel(pwmChannel);
+    if (index < 0) {
+      throw new NoSuchElementException("no addressable LED found for PWM channel " + pwmChannel);
+    }
+    return new AddressableLEDSim(index);
+  }
+
+  /**
+   * Creates an AddressableLEDSim for a simulated index.
+   * The index is incremented for each simulated AddressableLED.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  public static AddressableLEDSim createForIndex(int index) {
+    return new AddressableLEDSim(index);
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AddressableLEDDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return AddressableLEDDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    AddressableLEDDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerOutputPortCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AddressableLEDDataJNI.registerOutputPortCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelOutputPortCallback);
+  }
+  public int getOutputPort() {
+    return AddressableLEDDataJNI.getOutputPort(m_index);
+  }
+  public void setOutputPort(int outputPort) {
+    AddressableLEDDataJNI.setOutputPort(m_index, outputPort);
+  }
+
+  public CallbackStore registerLengthCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AddressableLEDDataJNI.registerLengthCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelLengthCallback);
+  }
+  public int getLength() {
+    return AddressableLEDDataJNI.getLength(m_index);
+  }
+  public void setLength(int length) {
+    AddressableLEDDataJNI.setLength(m_index, length);
+  }
+
+  public CallbackStore registerRunningCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AddressableLEDDataJNI.registerRunningCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelRunningCallback);
+  }
+  public boolean getRunning() {
+    return AddressableLEDDataJNI.getRunning(m_index);
+  }
+  public void setRunning(boolean running) {
+    AddressableLEDDataJNI.setRunning(m_index, running);
+  }
+
+  public CallbackStore registerDataCallback(ConstBufferCallback callback) {
+    int uid = AddressableLEDDataJNI.registerDataCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelDataCallback);
+  }
+  public byte[] getData() {
+    return AddressableLEDDataJNI.getData(m_index);
+  }
+  public void setData(byte[] data) {
+    AddressableLEDDataJNI.setData(m_index, data);
+  }
+
+  public void resetData() {
+    AddressableLEDDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java
new file mode 100644
index 0000000..e1b541f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Class to control a simulated analog encoder.
+ */
+public class AnalogEncoderSim {
+  private final SimDouble m_simPosition;
+
+  /**
+   * Constructs from an AnalogEncoder object.
+   *
+   * @param encoder AnalogEncoder to simulate
+   */
+  public AnalogEncoderSim(AnalogEncoder encoder) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]");
+    m_simPosition = wrappedSimDevice.getDouble("Position");
+  }
+
+  /**
+   * Set the position using an {@link Rotation2d}.
+   *
+   * @param angle The angle.
+   */
+  public void setPosition(Rotation2d angle) {
+    setTurns(angle.getDegrees() / 360.0);
+  }
+
+  /**
+   * Set the position of the encoder.
+   *
+   * @param turns The position.
+   */
+  public void setTurns(double turns) {
+    m_simPosition.set(turns);
+  }
+
+  /**
+   * Get the simulated position.
+   */
+  public double getTurns() {
+    return m_simPosition.get();
+  }
+
+  /**
+   * Get the position as a {@link Rotation2d}.
+   */
+  public Rotation2d getPosition() {
+    return Rotation2d.fromDegrees(getTurns() * 360.0);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java
new file mode 100644
index 0000000..b204fae
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AnalogGyroDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.AnalogGyro;
+
+/**
+ * Class to control a simulated analog gyro.
+ */
+public class AnalogGyroSim {
+  private final int m_index;
+
+  /**
+   * Constructs from an AnalogGyro object.
+   *
+   * @param gyro AnalogGyro to simulate
+   */
+  public AnalogGyroSim(AnalogGyro gyro) {
+    m_index = gyro.getAnalogInput().getChannel();
+  }
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  public AnalogGyroSim(int channel) {
+    m_index = channel;
+  }
+
+  public CallbackStore registerAngleCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogGyroDataJNI.registerAngleCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelAngleCallback);
+  }
+  public double getAngle() {
+    return AnalogGyroDataJNI.getAngle(m_index);
+  }
+  public void setAngle(double angle) {
+    AnalogGyroDataJNI.setAngle(m_index, angle);
+  }
+
+  public CallbackStore registerRateCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogGyroDataJNI.registerRateCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelRateCallback);
+  }
+  public double getRate() {
+    return AnalogGyroDataJNI.getRate(m_index);
+  }
+  public void setRate(double rate) {
+    AnalogGyroDataJNI.setRate(m_index, rate);
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogGyroDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return AnalogGyroDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    AnalogGyroDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public void resetData() {
+    AnalogGyroDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java
new file mode 100644
index 0000000..1709d6a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java
@@ -0,0 +1,140 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AnalogInDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.AnalogInput;
+
+/**
+ * Class to control a simulated analog input.
+ */
+public class AnalogInputSim {
+  private final int m_index;
+
+  /**
+   * Constructs from an AnalogInput object.
+   *
+   * @param analogInput AnalogInput to simulate
+   */
+  public AnalogInputSim(AnalogInput analogInput) {
+    m_index = analogInput.getChannel();
+  }
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  public AnalogInputSim(int channel) {
+    m_index = channel;
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return AnalogInDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    AnalogInDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerAverageBitsCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerAverageBitsCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAverageBitsCallback);
+  }
+  public int getAverageBits() {
+    return AnalogInDataJNI.getAverageBits(m_index);
+  }
+  public void setAverageBits(int averageBits) {
+    AnalogInDataJNI.setAverageBits(m_index, averageBits);
+  }
+
+  public CallbackStore registerOversampleBitsCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerOversampleBitsCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelOversampleBitsCallback);
+  }
+  public int getOversampleBits() {
+    return AnalogInDataJNI.getOversampleBits(m_index);
+  }
+  public void setOversampleBits(int oversampleBits) {
+    AnalogInDataJNI.setOversampleBits(m_index, oversampleBits);
+  }
+
+  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelVoltageCallback);
+  }
+  public double getVoltage() {
+    return AnalogInDataJNI.getVoltage(m_index);
+  }
+  public void setVoltage(double voltage) {
+    AnalogInDataJNI.setVoltage(m_index, voltage);
+  }
+
+  public CallbackStore registerAccumulatorInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerAccumulatorInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorInitializedCallback);
+  }
+  public boolean getAccumulatorInitialized() {
+    return AnalogInDataJNI.getAccumulatorInitialized(m_index);
+  }
+  public void setAccumulatorInitialized(boolean accumulatorInitialized) {
+    AnalogInDataJNI.setAccumulatorInitialized(m_index, accumulatorInitialized);
+  }
+
+  public CallbackStore registerAccumulatorValueCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerAccumulatorValueCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorValueCallback);
+  }
+  public long getAccumulatorValue() {
+    return AnalogInDataJNI.getAccumulatorValue(m_index);
+  }
+  public void setAccumulatorValue(long accumulatorValue) {
+    AnalogInDataJNI.setAccumulatorValue(m_index, accumulatorValue);
+  }
+
+  public CallbackStore registerAccumulatorCountCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerAccumulatorCountCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorCountCallback);
+  }
+  public long getAccumulatorCount() {
+    return AnalogInDataJNI.getAccumulatorCount(m_index);
+  }
+  public void setAccumulatorCount(long accumulatorCount) {
+    AnalogInDataJNI.setAccumulatorCount(m_index, accumulatorCount);
+  }
+
+  public CallbackStore registerAccumulatorCenterCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerAccumulatorCenterCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorCenterCallback);
+  }
+  public int getAccumulatorCenter() {
+    return AnalogInDataJNI.getAccumulatorCenter(m_index);
+  }
+  public void setAccumulatorCenter(int accumulatorCenter) {
+    AnalogInDataJNI.setAccumulatorCenter(m_index, accumulatorCenter);
+  }
+
+  public CallbackStore registerAccumulatorDeadbandCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogInDataJNI.registerAccumulatorDeadbandCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorDeadbandCallback);
+  }
+  public int getAccumulatorDeadband() {
+    return AnalogInDataJNI.getAccumulatorDeadband(m_index);
+  }
+  public void setAccumulatorDeadband(int accumulatorDeadband) {
+    AnalogInDataJNI.setAccumulatorDeadband(m_index, accumulatorDeadband);
+  }
+
+  public void resetData() {
+    AnalogInDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java
new file mode 100644
index 0000000..077a2eb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AnalogOutDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.AnalogOutput;
+
+/**
+ * Class to control a simulated analog output.
+ */
+public class AnalogOutputSim {
+  private final int m_index;
+
+  /**
+   * Constructs from an AnalogOutput object.
+   *
+   * @param analogOutput AnalogOutput to simulate
+   */
+  public AnalogOutputSim(AnalogOutput analogOutput) {
+    m_index = analogOutput.getChannel();
+  }
+
+  /**
+   * Constructs from an analog output channel number.
+   *
+   * @param channel Channel number
+   */
+  public AnalogOutputSim(int channel) {
+    m_index = channel;
+  }
+
+  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogOutDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogOutDataJNI::cancelVoltageCallback);
+  }
+  public double getVoltage() {
+    return AnalogOutDataJNI.getVoltage(m_index);
+  }
+  public void setVoltage(double voltage) {
+    AnalogOutDataJNI.setVoltage(m_index, voltage);
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogOutDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogOutDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return AnalogOutDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    AnalogOutDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public void resetData() {
+    AnalogOutDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java
new file mode 100644
index 0000000..6916fdc
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AnalogTriggerDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.AnalogTrigger;
+import java.util.NoSuchElementException;
+
+/**
+ * Class to control a simulated analog trigger.
+ */
+public class AnalogTriggerSim {
+  private final int m_index;
+
+  /**
+   * Constructs from an AnalogTrigger object.
+   *
+   * @param analogTrigger AnalogTrigger to simulate
+   */
+  public AnalogTriggerSim(AnalogTrigger analogTrigger) {
+    m_index = analogTrigger.getIndex();
+  }
+
+  private AnalogTriggerSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates an AnalogTriggerSim for an analog input channel.
+   *
+   * @param channel analog input channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no AnalogTrigger is configured for that channel
+   */
+  public static AnalogTriggerSim createForChannel(int channel) {
+    int index = AnalogTriggerDataJNI.findForChannel(channel);
+    if (index < 0) {
+      throw new NoSuchElementException("no analog trigger found for channel " + channel);
+    }
+    return new AnalogTriggerSim(index);
+  }
+
+  /**
+   * Creates an AnalogTriggerSim for a simulated index.
+   * The index is incremented for each simulated AnalogTrigger.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  public static AnalogTriggerSim createForIndex(int index) {
+    return new AnalogTriggerSim(index);
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogTriggerDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return AnalogTriggerDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    AnalogTriggerDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerTriggerLowerBoundCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogTriggerDataJNI.registerTriggerLowerBoundCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelTriggerLowerBoundCallback);
+  }
+  public double getTriggerLowerBound() {
+    return AnalogTriggerDataJNI.getTriggerLowerBound(m_index);
+  }
+  public void setTriggerLowerBound(double triggerLowerBound) {
+    AnalogTriggerDataJNI.setTriggerLowerBound(m_index, triggerLowerBound);
+  }
+
+  public CallbackStore registerTriggerUpperBoundCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AnalogTriggerDataJNI.registerTriggerUpperBoundCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelTriggerUpperBoundCallback);
+  }
+  public double getTriggerUpperBound() {
+    return AnalogTriggerDataJNI.getTriggerUpperBound(m_index);
+  }
+  public void setTriggerUpperBound(double triggerUpperBound) {
+    AnalogTriggerDataJNI.setTriggerUpperBound(m_index, triggerUpperBound);
+  }
+
+  public void resetData() {
+    AnalogTriggerDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java
new file mode 100644
index 0000000..0150ed4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.RobotController;
+
+public final class BatterySim {
+  private BatterySim() {
+    // Utility class
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link RobotController#getBatteryVoltage()}
+   * method.
+   *
+   * @param nominalVoltage The nominal battery voltage. Usually 12v.
+   * @param resistanceOhms The forward resistance of the battery. Most batteries are at or
+   *                       below 20 milliohms.
+   * @param currents       The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  public static double calculateLoadedBatteryVoltage(double nominalVoltage,
+                                                     double resistanceOhms,
+                                                     double... currents) {
+    double retval = nominalVoltage;
+    for (var current : currents) {
+      retval -= current * resistanceOhms;
+    }
+    return retval;
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link RobotController#getBatteryVoltage()}
+   * method. This function assumes a nominal voltage of 12v and a resistance of 20 milliohms
+   * (0.020 ohms)
+   *
+   * @param currents The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  public static double calculateDefaultBatteryLoadedVoltage(double... currents) {
+    return calculateLoadedBatteryVoltage(12, 0.02, currents);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java
new file mode 100644
index 0000000..8905542
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.AccelerometerDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.BuiltInAccelerometer;
+
+/**
+ * Class to control a simulated built-in accelerometer.
+ */
+public class BuiltInAccelerometerSim {
+  private final int m_index;
+
+  /**
+   * Constructs for the first built-in accelerometer.
+   */
+  public BuiltInAccelerometerSim() {
+    m_index = 0;
+  }
+
+  /**
+   * Constructs from a BuiltInAccelerometer object.
+   *
+   * @param accel BuiltInAccelerometer to simulate
+   */
+  @SuppressWarnings("PMD.UnusedFormalParameter")
+  public BuiltInAccelerometerSim(BuiltInAccelerometer accel) {
+    m_index = 0;
+  }
+
+  public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelActiveCallback);
+  }
+  public boolean getActive() {
+    return AccelerometerDataJNI.getActive(m_index);
+  }
+  public void setActive(boolean active) {
+    AccelerometerDataJNI.setActive(m_index, active);
+  }
+
+  public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelRangeCallback);
+  }
+  public int getRange() {
+    return AccelerometerDataJNI.getRange(m_index);
+  }
+  public void setRange(int range) {
+    AccelerometerDataJNI.setRange(m_index, range);
+  }
+
+  public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelXCallback);
+  }
+  public double getX() {
+    return AccelerometerDataJNI.getX(m_index);
+  }
+  public void setX(double x) {
+    AccelerometerDataJNI.setX(m_index, x);
+  }
+
+  public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelYCallback);
+  }
+  public double getY() {
+    return AccelerometerDataJNI.getY(m_index);
+  }
+  public void setY(double y) {
+    AccelerometerDataJNI.setY(m_index, y);
+  }
+
+  public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = AccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelZCallback);
+  }
+  public double getZ() {
+    return AccelerometerDataJNI.getZ(m_index);
+  }
+  public void setZ(double z) {
+    AccelerometerDataJNI.setZ(m_index, z);
+  }
+
+  public void resetData() {
+    AccelerometerDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java
new file mode 100644
index 0000000..3df142f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+public class CallbackStore implements AutoCloseable {
+  interface CancelCallbackFunc {
+    void cancel(int index, int uid);
+  }
+
+  interface CancelCallbackChannelFunc {
+    void cancel(int index, int channel, int uid);
+  }
+
+  interface CancelCallbackNoIndexFunc {
+    void cancel(int uid);
+  }
+
+  public CallbackStore(int index, int uid, CancelCallbackFunc ccf) {
+    this.m_cancelType = kNormalCancel;
+    this.m_index = index;
+    this.m_uid = uid;
+    this.m_cancelCallback = ccf;
+  }
+
+  public CallbackStore(int index, int channel, int uid, CancelCallbackChannelFunc ccf) {
+    this.m_cancelType = kChannelCancel;
+    this.m_index = index;
+    this.m_uid = uid;
+    this.m_channel = channel;
+    this.m_cancelCallbackChannel = ccf;
+  }
+
+  public CallbackStore(int uid, CancelCallbackNoIndexFunc ccf) {
+    this.m_cancelType = kNoIndexCancel;
+    this.m_uid = uid;
+    this.m_cancelCallbackNoIndex = ccf;
+  }
+
+  private int m_index;
+  private int m_channel;
+  private final int m_uid;
+  private CancelCallbackFunc m_cancelCallback;
+  private CancelCallbackChannelFunc m_cancelCallbackChannel;
+  private CancelCallbackNoIndexFunc m_cancelCallbackNoIndex;
+  private static final int kNormalCancel = 0;
+  private static final int kChannelCancel = 1;
+  private static final int kNoIndexCancel = 2;
+  private int m_cancelType;
+
+  @Override
+  public void close() {
+    switch (m_cancelType) {
+      case kNormalCancel:
+        m_cancelCallback.cancel(m_index, m_uid);
+        break;
+      case kChannelCancel:
+        m_cancelCallbackChannel.cancel(m_index, m_channel, m_uid);
+        break;
+      case kNoIndexCancel:
+        m_cancelCallbackNoIndex.cancel(m_uid);
+        break;
+      default:
+        assert false;
+        break;
+    }
+    m_cancelType = -1;
+  }
+
+  @Override
+  protected void finalize() throws Throwable {
+    try {
+      if (m_cancelType >= 0) {
+        close();        // close open files
+      }
+    } finally {
+      super.finalize();
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java
new file mode 100644
index 0000000..758d7e4
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.DIODataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+
+/**
+ * Class to control a simulated digital input or output.
+ */
+public class DIOSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a DigitalInput object.
+   *
+   * @param input DigitalInput to simulate
+   */
+  public DIOSim(DigitalInput input) {
+    m_index = input.getChannel();
+  }
+
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param output DigitalOutput to simulate
+   */
+  public DIOSim(DigitalOutput output) {
+    m_index = output.getChannel();
+  }
+
+  /**
+   * Constructs from an digital I/O channel number.
+   *
+   * @param channel Channel number
+   */
+  public DIOSim(int channel) {
+    m_index = channel;
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return DIODataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    DIODataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerValueCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerValueCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelValueCallback);
+  }
+  public boolean getValue() {
+    return DIODataJNI.getValue(m_index);
+  }
+  public void setValue(boolean value) {
+    DIODataJNI.setValue(m_index, value);
+  }
+
+  public CallbackStore registerPulseLengthCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerPulseLengthCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelPulseLengthCallback);
+  }
+  public double getPulseLength() {
+    return DIODataJNI.getPulseLength(m_index);
+  }
+  public void setPulseLength(double pulseLength) {
+    DIODataJNI.setPulseLength(m_index, pulseLength);
+  }
+
+  public CallbackStore registerIsInputCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerIsInputCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelIsInputCallback);
+  }
+  public boolean getIsInput() {
+    return DIODataJNI.getIsInput(m_index);
+  }
+  public void setIsInput(boolean isInput) {
+    DIODataJNI.setIsInput(m_index, isInput);
+  }
+
+  public CallbackStore registerFilterIndexCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DIODataJNI.registerFilterIndexCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DIODataJNI::cancelFilterIndexCallback);
+  }
+  public int getFilterIndex() {
+    return DIODataJNI.getFilterIndex(m_index);
+  }
+  public void setFilterIndex(int filterIndex) {
+    DIODataJNI.setFilterIndex(m_index, filterIndex);
+  }
+
+  public void resetData() {
+    DIODataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
new file mode 100644
index 0000000..ab3d003
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
@@ -0,0 +1,328 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpiutil.math.numbers.N7;
+
+/**
+ * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with
+ * {@link #setInputs(double, double)}, call {@link #update(double)} to update the simulation,
+ * and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.simulation.Field2d} to
+ * visualize their robot on the Sim GUI's field.
+ *
+ *  <p>Our state-space system is:
+ *
+ *  <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r, voltError_l, voltError_r, headingError]]^T
+ *  in the field coordinate system (dist_* are wheel distances.)
+ *
+ *  <p>u = [[voltage_l, voltage_r]]^T This is typically the control input of the last timestep
+ *  from a LTVDiffDriveController.
+ *
+ *  <p>y = [[x, y, theta]]^T from a global measurement source(ex. vision),
+ *  or y = [[dist_l, dist_r, theta]] from encoders and gyro.
+ *
+ */
+public class DifferentialDrivetrainSim {
+  private final DCMotor m_motor;
+  private final double m_originalGearing;
+  private double m_currentGearing;
+  private final double m_wheelRadiusMeters;
+  @SuppressWarnings("MemberName")
+  private Matrix<N2, N1> m_u;
+  @SuppressWarnings("MemberName")
+  private Matrix<N7, N1> m_x;
+
+  private final double m_rb;
+  private final LinearSystem<N2, N2, N2> m_plant;
+
+  /**
+   * Create a SimDrivetrain.
+   *
+   * @param driveMotor        A {@link DCMotor} representing the left side of the drivetrain.
+   * @param gearing           The gearing on the drive between motor and wheel, as output over input.
+   *                          This must be the same ratio as the ratio used to identify or
+   *                          create the drivetrainPlant.
+   * @param jKgMetersSquared  The moment of inertia of the drivetrain about its center.
+   * @param massKg            The mass of the drivebase.
+   * @param wheelRadiusMeters The radius of the wheels on the drivetrain.
+   * @param trackWidthMeters  The robot's track width, or distance between left and right wheels.
+   */
+  public DifferentialDrivetrainSim(DCMotor driveMotor, double gearing,
+                                   double jKgMetersSquared, double massKg,
+                                   double wheelRadiusMeters, double trackWidthMeters) {
+    this(LinearSystemId.createDrivetrainVelocitySystem(driveMotor, massKg, wheelRadiusMeters,
+        trackWidthMeters / 2.0, jKgMetersSquared, gearing),
+        driveMotor, gearing, trackWidthMeters, wheelRadiusMeters);
+  }
+
+  /**
+   * Create a SimDrivetrain
+   * .
+   * @param drivetrainPlant   The {@link LinearSystem} representing the robot's drivetrain. This
+   *                          system can be created with {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, double, double, double, double, double)}
+   *                          or {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double, double, double)}.
+   * @param driveMotor        A {@link DCMotor} representing the drivetrain.
+   * @param gearing           The gearingRatio ratio of the robot, as output over input.
+   *                          This must be the same ratio as the ratio used to identify or
+   *                          create the drivetrainPlant.
+   * @param trackWidthMeters  The distance between the two sides of the drivetrian. Can be
+   *                          found with frc-characterization.
+   * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters.
+   */
+  public DifferentialDrivetrainSim(LinearSystem<N2, N2, N2> drivetrainPlant,
+                                   DCMotor driveMotor, double gearing,
+                                   double trackWidthMeters,
+                                   double wheelRadiusMeters) {
+    this.m_plant = drivetrainPlant;
+    this.m_rb = trackWidthMeters / 2.0;
+    this.m_motor = driveMotor;
+    this.m_originalGearing = gearing;
+    m_wheelRadiusMeters = wheelRadiusMeters;
+    m_currentGearing = m_originalGearing;
+
+    m_x = new Matrix<>(Nat.N7(), Nat.N1());
+    m_u = VecBuilder.fill(0, 0);
+  }
+
+  /**
+   * Sets the applied voltage to the drivetrain. Note that positive voltage must make that
+   * side of the drivetrain travel forward (+X).
+   *
+   * @param leftVoltageVolts  The left voltage.
+   * @param rightVoltageVolts The right voltage.
+   */
+  public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
+    m_u = VecBuilder.fill(leftVoltageVolts, rightVoltageVolts);
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  public void update(double dtSeconds) {
+
+    // Update state estimate with RK4
+    m_x = RungeKutta.rungeKutta(this::getDynamics, m_x, m_u, dtSeconds);
+  }
+
+  public double getState(State state) {
+    return m_x.get(state.value, 0);
+  }
+
+  /**
+   * Returns the full simulated state of the drivetrain.
+   */
+  public Matrix<N7, N1> getState() {
+    return m_x;
+  }
+
+  /**
+   * Returns the direction the robot is pointing.
+   *
+   * <p>Note that this angle is counterclockwise-positive, while most gyros are
+   * clockwise positive.
+   */
+  public Rotation2d getHeading() {
+    return new Rotation2d(getState(State.kHeading));
+  }
+
+  /**
+   * Returns the current pose.
+   */
+  public Pose2d getPose() {
+    return new Pose2d(m_x.get(0, 0),
+      m_x.get(1, 0),
+      new Rotation2d(m_x.get(2, 0)));
+  }
+
+  public double getCurrentDrawAmps() {
+    var loadIleft = m_motor.getCurrent(
+        getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters,
+        m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
+
+    var loadIright = m_motor.getCurrent(
+        getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters,
+        m_u.get(1, 0)) * Math.signum(m_u.get(1, 0));
+
+    return loadIleft + loadIright;
+  }
+
+  public double getCurrentGearing() {
+    return m_currentGearing;
+  }
+
+  /**
+   * Sets the gearing reduction on the drivetrain. This is commonly used for
+   * shifting drivetrains.
+   *
+   * @param newGearRatio The new gear ratio, as output over input.
+   */
+  public void setCurrentGearing(double newGearRatio) {
+    this.m_currentGearing = newGearRatio;
+  }
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The state.
+   */
+  public void setState(Matrix<N7, N1> state) {
+    m_x = state;
+  }
+
+  /**
+   * Sets the system pose.
+   *
+   * @param pose The pose.
+   */
+  public void setPose(Pose2d pose) {
+    m_x.set(State.kX.value, 0, pose.getX());
+    m_x.set(State.kY.value, 0, pose.getY());
+    m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians());
+    m_x.set(State.kLeftPosition.value, 0, 0);
+    m_x.set(State.kRightPosition.value, 0, 0);
+  }
+
+  @SuppressWarnings({"DuplicatedCode", "LocalVariableName"})
+  protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> u) {
+
+    // Because G can be factored out of B, we can divide by the old ratio and multiply
+    // by the new ratio to get a new drivetrain model.
+    var B = new Matrix<>(Nat.N4(), Nat.N2());
+    B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing));
+
+    // Because G^2 can be factored out of A, we can divide by the old ratio squared and multiply
+    // by the new ratio squared to get a new drivetrain model.
+    var A = new Matrix<>(Nat.N4(), Nat.N4());
+    A.assignBlock(0, 0, m_plant.getA().times((this.m_currentGearing * this.m_currentGearing)
+        / (this.m_originalGearing * this.m_originalGearing)));
+
+    A.assignBlock(2, 0, Matrix.eye(Nat.N2()));
+
+    var v = (x.get(State.kLeftVelocity.value, 0) + x.get(State.kRightVelocity.value, 0)) / 2.0;
+
+    var xdot = new Matrix<>(Nat.N7(), Nat.N1());
+    xdot.set(0, 0, v * Math.cos(x.get(State.kHeading.value, 0)));
+    xdot.set(1, 0, v * Math.sin(x.get(State.kHeading.value, 0)));
+    xdot.set(2, 0, (x.get(State.kRightVelocity.value, 0)
+        - x.get(State.kLeftVelocity.value, 0)) / (2.0 * m_rb));
+    xdot.assignBlock(3, 0,
+        A.times(x.block(Nat.N4(), Nat.N1(), 3, 0))
+        .plus(B.times(u)));
+
+    return xdot;
+  }
+
+  public enum State {
+    kX(0),
+    kY(1),
+    kHeading(2),
+    kLeftVelocity(3),
+    kRightVelocity(4),
+    kLeftPosition(5),
+    kRightPosition(6);
+
+    @SuppressWarnings("MemberName")
+    public final int value;
+
+    State(int i) {
+      this.value = i;
+    }
+  }
+
+  /**
+   * Represents a gearing option of the Toughbox mini.
+   * 12.75:1 -- 14:50 and 14:50
+   * 10.71:1 -- 14:50 and 16:48
+   * 8.45:1 -- 14:50 and 19:45
+   * 7.31:1 -- 14:50 and 21:43
+   * 5.95:1 -- 14:50 and 24:40
+   */
+  public enum KitbotGearing {
+    k12p75(12.75),
+    k10p71(10.71),
+    k8p45(8.45),
+    k7p31(7.31),
+    k5p95(5.95);
+
+    @SuppressWarnings("MemberName")
+    public final double value;
+
+    KitbotGearing(double i) {
+      this.value = i;
+    }
+  }
+
+  public enum KitbotMotor {
+    kSingleCIMPerSide(DCMotor.getCIM(1)),
+    kDualCIMPerSide(DCMotor.getCIM(2)),
+    kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)),
+    kDualMiniCIMPerSide(DCMotor.getMiniCIM(2));
+
+    @SuppressWarnings("MemberName")
+    public final DCMotor value;
+
+    KitbotMotor(DCMotor i) {
+      this.value = i;
+    }
+  }
+
+  public enum KitbotWheelSize {
+    SixInch(Units.inchesToMeters(6)),
+    EightInch(Units.inchesToMeters(8)),
+    TenInch(Units.inchesToMeters(10));
+
+    @SuppressWarnings("MemberName")
+    public final double value;
+
+    KitbotWheelSize(double i) {
+      this.value = i;
+    }
+  }
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor     The motors installed in the bot.
+   * @param gearing   The gearing reduction used.
+   * @param wheelSize The wheel size.
+   */
+  public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
+                                                          KitbotWheelSize wheelSize) {
+    // MOI estimation -- note that I = m r^2 for point masses
+    var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2);
+    var gearboxMoi = (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */)
+        * Math.pow(Units.inchesToMeters(26.0 / 2.0), 2);
+
+    return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi);
+  }
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor            The motors installed in the bot.
+   * @param gearing          The gearing reduction used.
+   * @param wheelSize        The wheel size.
+   * @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using
+   *                         frc-characterization.
+   */
+  public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
+                                                          KitbotWheelSize wheelSize, double jKgMetersSquared) {
+    return new DifferentialDrivetrainSim(motor.value, gearing.value, jKgMetersSquared, 25 / 2.2,
+        wheelSize.value / 2.0, Units.inchesToMeters(26));
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java
new file mode 100644
index 0000000..db942eb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.DigitalPWMDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import java.util.NoSuchElementException;
+
+/**
+ * Class to control a simulated digital PWM output.
+ *
+ * <p>This is for duty cycle PWM outputs on a DigitalOutput, not for the servo
+ * style PWM outputs on a PWM channel.
+ */
+public class DigitalPWMSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param digitalOutput DigitalOutput to simulate
+   */
+  public DigitalPWMSim(DigitalOutput digitalOutput) {
+    m_index = digitalOutput.getChannel();
+  }
+
+  private DigitalPWMSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates an DigitalPWMSim for a digital I/O channel.
+   *
+   * @param channel DIO channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no Digital PWM is configured for that channel
+   */
+  public static DigitalPWMSim createForChannel(int channel) {
+    int index = DigitalPWMDataJNI.findForChannel(channel);
+    if (index < 0) {
+      throw new NoSuchElementException("no digital PWM found for channel " + channel);
+    }
+    return new DigitalPWMSim(index);
+  }
+
+  /**
+   * Creates an DigitalPWMSim for a simulated index.
+   * The index is incremented for each simulated DigitalPWM.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  public static DigitalPWMSim createForIndex(int index) {
+    return new DigitalPWMSim(index);
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DigitalPWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return DigitalPWMDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    DigitalPWMDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerDutyCycleCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DigitalPWMDataJNI.registerDutyCycleCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelDutyCycleCallback);
+  }
+  public double getDutyCycle() {
+    return DigitalPWMDataJNI.getDutyCycle(m_index);
+  }
+  public void setDutyCycle(double dutyCycle) {
+    DigitalPWMDataJNI.setDutyCycle(m_index, dutyCycle);
+  }
+
+  public CallbackStore registerPinCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DigitalPWMDataJNI.registerPinCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelPinCallback);
+  }
+  public int getPin() {
+    return DigitalPWMDataJNI.getPin(m_index);
+  }
+  public void setPin(int pin) {
+    DigitalPWMDataJNI.setPin(m_index, pin);
+  }
+
+  public void resetData() {
+    DigitalPWMDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java
new file mode 100644
index 0000000..40b1017
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java
@@ -0,0 +1,373 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.simulation.DriverStationDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * Class to control a simulated driver station.
+ */
+@SuppressWarnings({"PMD.UseUtilityClass", "PMD.GodClass", "PMD.ExcessivePublicCount"})
+public class DriverStationSim {
+  public static CallbackStore registerEnabledCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerEnabledCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelEnabledCallback);
+  }
+  public static boolean getEnabled() {
+    return DriverStationDataJNI.getEnabled();
+  }
+  public static void setEnabled(boolean enabled) {
+    DriverStationDataJNI.setEnabled(enabled);
+  }
+
+  public static CallbackStore registerAutonomousCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerAutonomousCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelAutonomousCallback);
+  }
+  public static boolean getAutonomous() {
+    return DriverStationDataJNI.getAutonomous();
+  }
+  public static void setAutonomous(boolean autonomous) {
+    DriverStationDataJNI.setAutonomous(autonomous);
+  }
+
+  public static CallbackStore registerTestCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerTestCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelTestCallback);
+  }
+  public static boolean getTest() {
+    return DriverStationDataJNI.getTest();
+  }
+  public static void setTest(boolean test) {
+    DriverStationDataJNI.setTest(test);
+  }
+
+  public static CallbackStore registerEStopCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerEStopCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelEStopCallback);
+  }
+  public static boolean getEStop() {
+    return DriverStationDataJNI.getEStop();
+  }
+  public static void setEStop(boolean eStop) {
+    DriverStationDataJNI.setEStop(eStop);
+  }
+
+  public static CallbackStore registerFmsAttachedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerFmsAttachedCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelFmsAttachedCallback);
+  }
+  public static boolean getFmsAttached() {
+    return DriverStationDataJNI.getFmsAttached();
+  }
+  public static void setFmsAttached(boolean fmsAttached) {
+    DriverStationDataJNI.setFmsAttached(fmsAttached);
+  }
+
+  public static CallbackStore registerDsAttachedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerDsAttachedCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelDsAttachedCallback);
+  }
+  public static boolean getDsAttached() {
+    return DriverStationDataJNI.getDsAttached();
+  }
+  public static void setDsAttached(boolean dsAttached) {
+    DriverStationDataJNI.setDsAttached(dsAttached);
+  }
+
+  public static CallbackStore registerAllianceStationIdCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerAllianceStationIdCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelAllianceStationIdCallback);
+  }
+  public static AllianceStationID getAllianceStationId() {
+    switch (DriverStationDataJNI.getAllianceStationId()) {
+      case 0:
+        return AllianceStationID.Red1;
+      case 1:
+        return AllianceStationID.Red2;
+      case 2:
+        return AllianceStationID.Red3;
+      case 3:
+        return AllianceStationID.Blue1;
+      case 4:
+        return AllianceStationID.Blue2;
+      case 5:
+        return AllianceStationID.Blue3;
+      default:
+        return null;
+    }
+  }
+  public static void setAllianceStationId(AllianceStationID allianceStationId) {
+    int allianceStation;
+    switch (allianceStationId) {
+      case Red1:
+        allianceStation = 0;
+        break;
+      case Red2:
+        allianceStation = 1;
+        break;
+      case Red3:
+        allianceStation = 2;
+        break;
+      case Blue1:
+        allianceStation = 3;
+        break;
+      case Blue2:
+        allianceStation = 4;
+        break;
+      case Blue3:
+        allianceStation = 5;
+        break;
+      default:
+        return;
+    }
+    DriverStationDataJNI.setAllianceStationId(allianceStation);
+  }
+
+  public static CallbackStore registerMatchTimeCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DriverStationDataJNI.registerMatchTimeCallback(callback, initialNotify);
+    return new CallbackStore(uid, DriverStationDataJNI::cancelMatchTimeCallback);
+  }
+  public static double getMatchTime() {
+    return DriverStationDataJNI.getMatchTime();
+  }
+  public static void setMatchTime(double matchTime) {
+    DriverStationDataJNI.setMatchTime(matchTime);
+  }
+
+  /**
+   * Updates DriverStation data so that new values are visible to the user
+   * program.
+   */
+  public static void notifyNewData() {
+    DriverStationDataJNI.notifyNewData();
+    DriverStation.getInstance().waitForData();
+  }
+
+  /**
+   * Sets suppression of DriverStation.reportError and reportWarning messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  public static void setSendError(boolean shouldSend) {
+    DriverStationDataJNI.setSendError(shouldSend);
+  }
+
+  /**
+   * Sets suppression of DriverStation.sendConsoleLine messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  public static void setSendConsoleLine(boolean shouldSend) {
+    DriverStationDataJNI.setSendConsoleLine(shouldSend);
+  }
+
+  /**
+   * Gets the joystick outputs.
+   *
+   * @param stick The joystick number
+   * @return The joystick outputs
+   */
+  public static long getJoystickOutputs(int stick) {
+    return DriverStationDataJNI.getJoystickOutputs(stick);
+  }
+
+  /**
+   * Gets the joystick rumble.
+   *
+   * @param stick The joystick number
+   * @param rumbleNum Rumble to get (0=left, 1=right)
+   * @return The joystick rumble value
+   */
+  public static int getJoystickRumble(int stick, int rumbleNum) {
+    return DriverStationDataJNI.getJoystickRumble(stick, rumbleNum);
+  }
+
+  /**
+   * Sets the state of one joystick button. Button indexes begin at 1.
+   *
+   * @param stick  The joystick number
+   * @param button The button index, beginning at 1
+   * @param state The state of the joystick button
+   */
+  public static void setJoystickButton(int stick, int button, boolean state) {
+    DriverStationDataJNI.setJoystickButton(stick, button, state);
+  }
+
+  /**
+   * Gets the value of the axis on a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis  The analog axis number
+   * @param value The value of the axis on the joystick
+   */
+  public static void setJoystickAxis(int stick, int axis, double value) {
+    DriverStationDataJNI.setJoystickAxis(stick, axis, value);
+  }
+
+  /**
+   * Gets the state of a POV on a joystick.
+   *
+   * @param stick The joystick number
+   * @param pov The POV number
+   * @param value the angle of the POV in degrees, or -1 for not pressed
+   */
+  public static void setJoystickPOV(int stick, int pov, int value) {
+    DriverStationDataJNI.setJoystickPOV(stick, pov, value);
+  }
+
+  /**
+   * Sets the state of all the buttons on a joystick.
+   *
+   * @param stick The joystick number
+   * @param buttons The bitmap state of the buttons on the joystick
+   */
+  public static void setJoystickButtons(int stick, int buttons) {
+    DriverStationDataJNI.setJoystickButtonsValue(stick, buttons);
+  }
+
+  /**
+   * Sets the number of axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of axes on the indicated joystick
+   */
+  public static void setJoystickAxisCount(int stick, int count) {
+    DriverStationDataJNI.setJoystickAxisCount(stick, count);
+  }
+
+  /**
+   * Sets the number of POVs for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of POVs on the indicated joystick
+   */
+  public static void setJoystickPOVCount(int stick, int count) {
+    DriverStationDataJNI.setJoystickPOVCount(stick, count);
+  }
+
+  /**
+   * Sets the number of buttons for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of buttons on the indicated joystick
+   */
+  public static void setJoystickButtonCount(int stick, int count) {
+    DriverStationDataJNI.setJoystickButtonCount(stick, count);
+  }
+
+  /**
+   * Sets the value of isXbox for a joystick.
+   *
+   * @param stick The joystick number
+   * @param isXbox The value of isXbox
+   */
+  public static void setJoystickIsXbox(int stick, boolean isXbox) {
+    DriverStationDataJNI.setJoystickIsXbox(stick, isXbox);
+  }
+
+  /**
+   * Sets the value of type for a joystick.
+   *
+   * @param stick The joystick number
+   * @param type The value of type
+   */
+  public static void setJoystickType(int stick, int type) {
+    DriverStationDataJNI.setJoystickType(stick, type);
+  }
+
+  /**
+   * Sets the name of a joystick.
+   *
+   * @param stick The joystick number
+   * @param name The value of name
+   */
+  public static void setJoystickName(int stick, String name) {
+    DriverStationDataJNI.setJoystickName(stick, name);
+  }
+
+  /**
+   * Sets the types of Axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The target axis
+   * @param type The type of axis
+   */
+  public static void setJoystickAxisType(int stick, int axis, int type) {
+    DriverStationDataJNI.setJoystickAxisType(stick, axis, type);
+  }
+
+  /**
+   * Sets the game specific message.
+   *
+   * @param message the game specific message
+   */
+  public static void setGameSpecificMessage(String message) {
+    DriverStationDataJNI.setGameSpecificMessage(message);
+  }
+
+  /**
+   * Sets the event name.
+   *
+   * @param name the event name
+   */
+  public static void setEventName(String name) {
+    DriverStationDataJNI.setEventName(name);
+  }
+
+  /**
+   * Sets the match type.
+   *
+   * @param type the match type
+   */
+  public static void setMatchType(DriverStation.MatchType type) {
+    int matchType;
+    switch (type) {
+      case Practice:
+        matchType = 1;
+        break;
+      case Qualification:
+        matchType = 2;
+        break;
+      case Elimination:
+        matchType = 3;
+        break;
+      case None:
+        matchType = 0;
+        break;
+      default:
+        return;
+    }
+    DriverStationDataJNI.setMatchType(matchType);
+  }
+
+  /**
+   * Sets the match number.
+   *
+   * @param matchNumber the match number
+   */
+  public static void setMatchNumber(int matchNumber) {
+    DriverStationDataJNI.setMatchNumber(matchNumber);
+  }
+
+  /**
+   * Sets the replay number.
+   *
+   * @param replayNumber the replay number
+   */
+  public static void setReplayNumber(int replayNumber) {
+    DriverStationDataJNI.setReplayNumber(replayNumber);
+  }
+
+  public static void resetData() {
+    DriverStationDataJNI.resetData();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java
new file mode 100644
index 0000000..92b0883
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+/**
+ * Class to control a simulated duty cycle encoder.
+ */
+public class DutyCycleEncoderSim {
+  private final SimDouble m_simPosition;
+  private final SimDouble m_simDistancePerRotation;
+
+  /**
+   * Constructs from an DutyCycleEncoder object.
+   *
+   * @param encoder DutyCycleEncoder to simulate
+   */
+  public DutyCycleEncoderSim(DutyCycleEncoder encoder) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("DutyCycleEncoder" + "[" + encoder.getFPGAIndex() + "]");
+    m_simPosition = wrappedSimDevice.getDouble("Position");
+    m_simDistancePerRotation = wrappedSimDevice.getDouble("DistancePerRotation");
+  }
+
+  /**
+   * Set the position in turns.
+   *
+   * @param turns The position.
+   */
+  public void set(double turns) {
+    m_simPosition.set(turns);
+  }
+
+  /**
+   * Set the position.
+   */
+  public void setDistance(double distance) {
+    m_simPosition.set(distance / m_simDistancePerRotation.get());
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java
new file mode 100644
index 0000000..726e08c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.DutyCycleDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.DutyCycle;
+import java.util.NoSuchElementException;
+
+/**
+ * Class to control a simulated duty cycle digital input.
+ */
+public class DutyCycleSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a DutyCycle object.
+   *
+   * @param dutyCycle DutyCycle to simulate
+   */
+  public DutyCycleSim(DutyCycle dutyCycle) {
+    m_index = dutyCycle.getFPGAIndex();
+  }
+
+  private DutyCycleSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates a DutyCycleSim for a digital input channel.
+   *
+   * @param channel digital input channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no DutyCycle is configured for that channel
+   */
+  public static DutyCycleSim createForChannel(int channel) {
+    int index = DutyCycleDataJNI.findForChannel(channel);
+    if (index < 0) {
+      throw new NoSuchElementException("no duty cycle found for channel " + channel);
+    }
+    return new DutyCycleSim(index);
+  }
+
+  /**
+   * Creates a DutyCycleSim for a simulated index.
+   * The index is incremented for each simulated DutyCycle.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  public static DutyCycleSim createForIndex(int index) {
+    return new DutyCycleSim(index);
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DutyCycleDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return DutyCycleDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    DutyCycleDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerFrequencyCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DutyCycleDataJNI.registerFrequencyCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelFrequencyCallback);
+  }
+  public int getFrequency() {
+    return DutyCycleDataJNI.getFrequency(m_index);
+  }
+  public void setFrequency(int frequency) {
+    DutyCycleDataJNI.setFrequency(m_index, frequency);
+  }
+
+  public CallbackStore registerOutputCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = DutyCycleDataJNI.registerOutputCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelOutputCallback);
+  }
+  public double getOutput() {
+    return DutyCycleDataJNI.getOutput(m_index);
+  }
+  public void setOutput(double output) {
+    DutyCycleDataJNI.setOutput(m_index, output);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
new file mode 100644
index 0000000..1610089
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
@@ -0,0 +1,201 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+/**
+ * Represents a simulated elevator mechanism.
+ */
+public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
+  // Gearbox for the elevator.
+  private final DCMotor m_gearbox;
+
+  // Gearing between the motors and the output.
+  private final double m_gearing;
+
+  // The radius of the drum that the elevator spool is wrapped around.
+  private final double m_drumRadius;
+
+  // The min allowable height for the elevator.
+  private final double m_minHeight;
+
+  // The max allowable height for the elevator.
+  private final double m_maxHeight;
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param plant            The linear system that represents the elevator.
+   * @param gearbox          The type of and number of motors in the elevator gearbox.
+   * @param gearing          The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+   * @param minHeightMeters  The min allowable height of the elevator.
+   * @param maxHeightMeters  The max allowable height of the elevator.
+   */
+  public ElevatorSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
+                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) {
+    this(plant, gearbox, gearing, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
+  }
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param plant              The linear system that represents the elevator.
+   * @param gearbox            The type of and number of motors in the elevator gearbox.
+   * @param gearing            The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param drumRadiusMeters   The radius of the drum that the elevator spool is wrapped around.
+   * @param minHeightMeters    The min allowable height of the elevator.
+   * @param maxHeightMeters    The max allowable height of the elevator.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public ElevatorSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
+                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters,
+                     Matrix<N1, N1> measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+    m_drumRadius = drumRadiusMeters;
+    m_minHeight = minHeightMeters;
+    m_maxHeight = maxHeightMeters;
+  }
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param gearbox          The type of and number of motors in the elevator gearbox.
+   * @param gearing          The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param carriageMassKg   The mass of the elevator carriage.
+   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+   * @param minHeightMeters  The min allowable height of the elevator.
+   * @param maxHeightMeters  The max allowable height of the elevator.
+   */
+  public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg,
+                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) {
+    this(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters,
+        null);
+  }
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param gearbox            The type of and number of motors in the elevator gearbox.
+   * @param gearing            The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param carriageMassKg     The mass of the elevator carriage.
+   * @param drumRadiusMeters   The radius of the drum that the elevator spool is wrapped around.
+   * @param minHeightMeters    The min allowable height of the elevator.
+   * @param maxHeightMeters    The max allowable height of the elevator.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg,
+                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters,
+                     Matrix<N1, N1> measurementStdDevs) {
+    super(LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
+        measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+    m_drumRadius = drumRadiusMeters;
+    m_minHeight = minHeightMeters;
+    m_maxHeight = maxHeightMeters;
+  }
+
+  /**
+   * Returns whether the elevator has hit the lower limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the lower limit.
+   */
+  public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
+    return x.get(0, 0) < this.m_minHeight;
+  }
+
+  /**
+   * Returns whether the elevator has hit the upper limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the upper limit.
+   */
+  public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
+    return x.get(0, 0) > this.m_maxHeight;
+  }
+
+  /**
+   * Returns the position of the elevator.
+   *
+   * @return The position of the elevator.
+   */
+  public double getPositionMeters() {
+    return getOutput(0);
+  }
+
+  /**
+   * Returns the velocity of the elevator.
+   *
+   * @return The velocity of the elevator.
+   */
+  public double getVelocityMetersPerSecond() {
+    return m_x.get(0, 1);
+  }
+
+  /**
+   * Returns the elevator current draw.
+   *
+   * @return The elevator current draw.
+   */
+  @Override
+  public double getCurrentDrawAmps() {
+    // I = V / R - omega / (Kv * R)
+    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
+    // spinning 10x faster than the output
+    // v = r w, so w = v/r
+    double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing;
+    var appliedVoltage = m_u.get(0, 0);
+    return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage);
+  }
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+  }
+
+  /**
+   * Updates the state of the elevator.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dtSeconds   The time difference between controller updates.
+   */
+  @Override
+  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat,
+                                   Matrix<N1, N1> u, double dtSeconds) {
+    // Calculate updated x-hat from Runge-Kutta.
+    var updatedXhat = RungeKutta.rungeKutta(
+        (x, u_) -> (m_plant.getA().times(x)).plus(m_plant.getB().times(u_))
+            .plus(VecBuilder.fill(0, -9.8)), currentXhat, u, dtSeconds);
+
+    // We check for collisions after updating x-hat.
+    if (hasHitLowerLimit(updatedXhat)) {
+      return VecBuilder.fill(m_minHeight, 0);
+    }
+    if (hasHitUpperLimit(updatedXhat)) {
+      return VecBuilder.fill(m_maxHeight, 0);
+    }
+    return updatedXhat;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java
new file mode 100644
index 0000000..9df5187
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java
@@ -0,0 +1,168 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.EncoderDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.Encoder;
+import java.util.NoSuchElementException;
+
+/**
+ * Class to control a simulated encoder.
+ */
+public class EncoderSim {
+  private final int m_index;
+
+  /**
+   * Constructs from an Encoder object.
+   *
+   * @param encoder Encoder to simulate
+   */
+  public EncoderSim(Encoder encoder) {
+    m_index = encoder.getFPGAIndex();
+  }
+
+  private EncoderSim(int index) {
+    m_index = index;
+  }
+
+  /**
+   * Creates an EncoderSim for a digital input channel.  Encoders take two
+   * channels, so either one may be specified.
+   *
+   * @param channel digital input channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no Encoder is configured for that channel
+   */
+  public static EncoderSim createForChannel(int channel) {
+    int index = EncoderDataJNI.findForChannel(channel);
+    if (index < 0) {
+      throw new NoSuchElementException("no encoder found for channel " + channel);
+    }
+    return new EncoderSim(index);
+  }
+
+  /**
+   * Creates an EncoderSim for a simulated index.
+   * The index is incremented for each simulated Encoder.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  public static EncoderSim createForIndex(int index) {
+    return new EncoderSim(index);
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return EncoderDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    EncoderDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerCountCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerCountCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelCountCallback);
+  }
+  public int getCount() {
+    return EncoderDataJNI.getCount(m_index);
+  }
+  public void setCount(int count) {
+    EncoderDataJNI.setCount(m_index, count);
+  }
+
+  public CallbackStore registerPeriodCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerPeriodCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelPeriodCallback);
+  }
+  public double getPeriod() {
+    return EncoderDataJNI.getPeriod(m_index);
+  }
+  public void setPeriod(double period) {
+    EncoderDataJNI.setPeriod(m_index, period);
+  }
+
+  public CallbackStore registerResetCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerResetCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelResetCallback);
+  }
+  public boolean getReset() {
+    return EncoderDataJNI.getReset(m_index);
+  }
+  public void setReset(boolean reset) {
+    EncoderDataJNI.setReset(m_index, reset);
+  }
+
+  public CallbackStore registerMaxPeriodCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerMaxPeriodCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelMaxPeriodCallback);
+  }
+  public double getMaxPeriod() {
+    return EncoderDataJNI.getMaxPeriod(m_index);
+  }
+  public void setMaxPeriod(double maxPeriod) {
+    EncoderDataJNI.setMaxPeriod(m_index, maxPeriod);
+  }
+
+  public CallbackStore registerDirectionCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerDirectionCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelDirectionCallback);
+  }
+  public boolean getDirection() {
+    return EncoderDataJNI.getDirection(m_index);
+  }
+  public void setDirection(boolean direction) {
+    EncoderDataJNI.setDirection(m_index, direction);
+  }
+
+  public CallbackStore registerReverseDirectionCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerReverseDirectionCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelReverseDirectionCallback);
+  }
+  public boolean getReverseDirection() {
+    return EncoderDataJNI.getReverseDirection(m_index);
+  }
+  public void setReverseDirection(boolean reverseDirection) {
+    EncoderDataJNI.setReverseDirection(m_index, reverseDirection);
+  }
+
+  public CallbackStore registerSamplesToAverageCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = EncoderDataJNI.registerSamplesToAverageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, EncoderDataJNI::cancelSamplesToAverageCallback);
+  }
+  public int getSamplesToAverage() {
+    return EncoderDataJNI.getSamplesToAverage(m_index);
+  }
+  public void setSamplesToAverage(int samplesToAverage) {
+    EncoderDataJNI.setSamplesToAverage(m_index, samplesToAverage);
+  }
+
+  public void setDistance(double distance) {
+    EncoderDataJNI.setDistance(m_index, distance);
+  }
+
+  public double getDistance() {
+    return EncoderDataJNI.getDistance(m_index);
+  }
+
+  public void setRate(double rate) {
+    EncoderDataJNI.setRate(m_index, rate);
+  }
+
+  public double getRate() {
+    return EncoderDataJNI.getRate(m_index);
+  }
+
+  public void resetData() {
+    EncoderDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java
new file mode 100644
index 0000000..bbda95a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * 2D representation of game field (for simulation).
+ *
+ * <p>In non-simulation mode this simply stores and returns the robot pose.
+ *
+ * <p>The robot pose is the actual location shown on the simulation view.
+ * This may or may not match the robot's internal odometry.  For example, if
+ * the robot is shown at a particular starting location, the pose in this
+ * class would represent the actual location on the field, but the robot's
+ * internal state might have a 0,0,0 pose (unless it's initialized to
+ * something different).
+ *
+ * <p>As the user is able to edit the pose, code performing updates should get
+ * the robot pose, transform it as appropriate (e.g. based on simulated wheel
+ * velocity), and set the new pose.
+ */
+public class Field2d {
+  public Field2d() {
+    m_device = SimDevice.create("Field2D");
+    if (m_device != null) {
+      m_x = m_device.createDouble("x", false, 0.0);
+      m_y = m_device.createDouble("y", false, 0.0);
+      m_rot = m_device.createDouble("rot", false, 0.0);
+    }
+  }
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  public void setRobotPose(Pose2d pose) {
+    if (m_device != null) {
+      Translation2d translation = pose.getTranslation();
+      m_x.set(translation.getX());
+      m_y.set(translation.getY());
+      m_rot.set(pose.getRotation().getDegrees());
+    } else {
+      m_pose = pose;
+    }
+  }
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param xMeters X location, in meters
+   * @param yMeters Y location, in meters
+   * @param rotation rotation
+   */
+  public void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
+    if (m_device != null) {
+      m_x.set(xMeters);
+      m_y.set(yMeters);
+      m_rot.set(rotation.getDegrees());
+    } else {
+      m_pose = new Pose2d(xMeters, yMeters, rotation);
+    }
+  }
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  public Pose2d getRobotPose() {
+    if (m_device != null) {
+      return new Pose2d(m_x.get(), m_y.get(), Rotation2d.fromDegrees(m_rot.get()));
+    } else {
+      return m_pose;
+    }
+  }
+
+  private Pose2d m_pose;
+
+  @SuppressWarnings("MemberName")
+  private final SimDevice m_device;
+
+  @SuppressWarnings("MemberName")
+  private SimDouble m_x;
+
+  @SuppressWarnings("MemberName")
+  private SimDouble m_y;
+
+  private SimDouble m_rot;
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
new file mode 100644
index 0000000..785facc
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * Represents a simulated flywheel mechanism.
+ */
+public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
+  // Gearbox for the flywheel.
+  private final DCMotor m_gearbox;
+
+  // The gearing from the motors to the output.
+  private final double m_gearing;
+
+  /**
+   * Creates a simulated flywheel mechanism.
+   *
+   * @param plant   The linear system that represents the flywheel.
+   * @param gearbox The type of and number of motors in the flywheel gearbox.
+   * @param gearing The gearing of the flywheel (numbers greater than 1
+   *                represent reductions).
+   */
+  public FlywheelSim(LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double gearing) {
+    super(plant);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Creates a simulated flywheel mechanism.
+   *
+   * @param plant              The linear system that represents the flywheel.
+   * @param gearbox            The type of and number of motors in the flywheel
+   *                           gearbox.
+   * @param gearing            The gearing of the flywheel (numbers greater
+   *                           than 1 represent reductions).
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public FlywheelSim(LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double gearing,
+                     Matrix<N1, N1> measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Creates a simulated flywheel mechanism.
+   *
+   * @param gearbox          The type of and number of motors in the flywheel gearbox.
+   * @param gearing          The gearing of the flywheel (numbers greater than 1
+   *                         represent reductions).
+   * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown,
+   *                         use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)}
+   *                         constructor.
+   */
+  public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
+    super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared));
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Creates a simulated flywheel mechanism.
+   *
+   * @param gearbox            The type of and number of motors in the flywheel
+   *                           gearbox.
+   * @param gearing            The gearing of the flywheel (numbers greater
+   *                           than 1 represent reductions).
+   * @param jKgMetersSquared   The moment of inertia of the flywheel. If this is unknown,
+   *                           use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)}
+   *                           constructor.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
+                     Matrix<N1, N1> measurementStdDevs) {
+    super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared),
+        measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+  }
+
+  /**
+   * Returns the flywheel velocity.
+   *
+   * @return The flywheel velocity.
+   */
+  public double getAngularVelocityRadPerSec() {
+    return getOutput(0);
+  }
+
+  /**
+   * Returns the flywheel velocity in RPM.
+   *
+   * @return The flywheel velocity in RPM.
+   */
+  public double getAngularVelocityRPM() {
+    return Units.radiansPerSecondToRotationsPerMinute(getOutput(0));
+  }
+
+  /**
+   * Returns the flywheel current draw.
+   *
+   * @return The flywheel current draw.
+   */
+  @Override
+  public double getCurrentDrawAmps() {
+    // I = V / R - omega / (Kv * R)
+    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
+    // 2x faster than the flywheel
+    return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
+        * Math.signum(m_u.get(0, 0));
+  }
+
+  /**
+   * Sets the input voltage for the flywheel.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
new file mode 100644
index 0000000..506be52
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * Class to control a simulated generic joystick.
+ */
+public class GenericHIDSim {
+  protected final int m_port;
+
+  /**
+   * Constructs from a GenericHID object.
+   *
+   * @param joystick joystick to simulate
+   */
+  public GenericHIDSim(GenericHID joystick) {
+    m_port = joystick.getPort();
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  public GenericHIDSim(int port) {
+    m_port = port;
+  }
+
+  /**
+   * Updates joystick data so that new values are visible to the user program.
+   */
+  public void notifyNewData() {
+    DriverStationSim.notifyNewData();
+  }
+
+  public void setRawButton(int button, boolean value) {
+    DriverStationSim.setJoystickButton(m_port, button, value);
+  }
+
+  public void setRawAxis(int axis, double value) {
+    DriverStationSim.setJoystickAxis(m_port, axis, value);
+  }
+
+  public void setPOV(int pov, int value) {
+    DriverStationSim.setJoystickPOV(m_port, pov, value);
+  }
+
+  public void setPOV(int value) {
+    setPOV(0, value);
+  }
+
+  public void setAxisCount(int count) {
+    DriverStationSim.setJoystickAxisCount(m_port, count);
+  }
+
+  public void setPOVCount(int count) {
+    DriverStationSim.setJoystickPOVCount(m_port, count);
+  }
+
+  public void setButtonCount(int count) {
+    DriverStationSim.setJoystickButtonCount(m_port, count);
+  }
+
+  public void setType(GenericHID.HIDType type) {
+    DriverStationSim.setJoystickType(m_port, type.value);
+  }
+
+  public void setName(String name) {
+    DriverStationSim.setJoystickName(m_port, name);
+  }
+
+  public void setAxisType(int axis, int type) {
+    DriverStationSim.setJoystickAxisType(m_port, axis, type);
+  }
+
+  public boolean getOutput(int outputNumber) {
+    long outputs = getOutputs();
+    return (outputs & (1 << (outputNumber - 1))) != 0;
+  }
+
+  public long getOutputs() {
+    return DriverStationSim.getJoystickOutputs(m_port);
+  }
+
+  public double getRumble(GenericHID.RumbleType type) {
+    int value = DriverStationSim.getJoystickRumble(
+        m_port, type == GenericHID.RumbleType.kLeftRumble ? 0 : 1);
+    return value / 65535.0;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java
new file mode 100644
index 0000000..1252eeb
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.BufferCallback;
+import edu.wpi.first.hal.simulation.ConstBufferCallback;
+import edu.wpi.first.hal.simulation.I2CDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+
+public class I2CSim {
+  private final int m_index;
+
+  public I2CSim(int index) {
+    m_index = index;
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = I2CDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, I2CDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return I2CDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    I2CDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerReadCallback(BufferCallback callback) {
+    int uid = I2CDataJNI.registerReadCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, I2CDataJNI::cancelReadCallback);
+  }
+
+  public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
+    int uid = I2CDataJNI.registerWriteCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, I2CDataJNI::cancelWriteCallback);
+  }
+
+  public void resetData() {
+    I2CDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
new file mode 100644
index 0000000..d1d7100
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+/**
+ * Class to control a simulated joystick.
+ */
+public class JoystickSim extends GenericHIDSim {
+  private Joystick m_joystick;
+
+  /**
+   * Constructs from a Joystick object.
+   *
+   * @param joystick joystick to simulate
+   */
+  public JoystickSim(Joystick joystick) {
+    super(joystick);
+    m_joystick = joystick;
+    // default to a reasonable joystick configuration
+    setAxisCount(5);
+    setButtonCount(12);
+    setPOVCount(1);
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  public JoystickSim(int port) {
+    super(port);
+    // default to a reasonable joystick configuration
+    setAxisCount(5);
+    setButtonCount(12);
+    setPOVCount(1);
+  }
+
+  public void setX(double value) {
+    setRawAxis(m_joystick != null ? m_joystick.getXChannel() : Joystick.kDefaultXChannel, value);
+  }
+
+  public void setY(double value) {
+    setRawAxis(m_joystick != null ? m_joystick.getYChannel() : Joystick.kDefaultYChannel, value);
+  }
+
+  public void setZ(double value) {
+    setRawAxis(m_joystick != null ? m_joystick.getZChannel() : Joystick.kDefaultZChannel, value);
+  }
+
+  public void setTwist(double value) {
+    setRawAxis(
+        m_joystick != null ? m_joystick.getTwistChannel() : Joystick.kDefaultTwistChannel,
+        value);
+  }
+
+  public void setThrottle(double value) {
+    setRawAxis(
+        m_joystick != null ? m_joystick.getThrottleChannel() : Joystick.kDefaultThrottleChannel,
+        value);
+  }
+
+  public void setTrigger(boolean state) {
+    setRawButton(1, state);
+  }
+
+  public void setTop(boolean state) {
+    setRawButton(2, state);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
new file mode 100644
index 0000000..dc69ffa
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
@@ -0,0 +1,176 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import org.ejml.MatrixDimensionException;
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * This class helps simulate linear systems. To use this class, do the following in the
+ * {@link edu.wpi.first.wpilibj.IterativeRobotBase#simulationPeriodic} method.
+ *
+ * <p>Call {@link #setInput(double...)} with the inputs to the system (usually voltage).
+ *
+ * <p>Call {@link #update} to update the simulation.
+ *
+ * <p>Set simulated sensor readings with the simulated positions in {@link #getOutput()}
+ *
+ * @param <States>  The number of states of the system.
+ * @param <Inputs>  The number of inputs to the system.
+ * @param <Outputs> The number of outputs of the system.
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs extends Num> {
+  // The plant that represents the linear system.
+  protected final LinearSystem<States, Inputs, Outputs> m_plant;
+
+  // Variables for state, output, and input.
+  @SuppressWarnings("MemberName")
+  protected Matrix<States, N1> m_x;
+  @SuppressWarnings("MemberName")
+  protected Matrix<Outputs, N1> m_y;
+  @SuppressWarnings("MemberName")
+  protected Matrix<Inputs, N1> m_u;
+
+  // The standard deviations of measurements, used for adding noise
+  // to the measurements.
+  protected final Matrix<Outputs, N1> m_measurementStdDevs;
+
+  /**
+   * Creates a simulated generic linear system.
+   *
+   * @param system The system to simulate.
+   */
+  public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system) {
+    this(system, null);
+  }
+
+  /**
+   * Creates a simulated generic linear system with measurement noise.
+   *
+   * @param system             The system being controlled.
+   * @param measurementStdDevs Standard deviations of measurements.
+   */
+  public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system,
+                         Matrix<Outputs, N1> measurementStdDevs) {
+    this.m_plant = system;
+    this.m_measurementStdDevs = measurementStdDevs;
+
+    m_x = new Matrix<>(new SimpleMatrix(system.getA().getNumRows(), 1));
+    m_u = new Matrix<>(new SimpleMatrix(system.getB().getNumCols(), 1));
+    m_y = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1));
+  }
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dtSeconds The time between updates.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public void update(double dtSeconds) {
+    // Update X. By default, this is the linear system dynamics X = Ax + Bu
+    m_x = updateX(m_x, m_u, dtSeconds);
+
+    // y = cx + du
+    m_y = m_plant.calculateY(m_x, m_u);
+
+    // Add measurement noise.
+    if (m_measurementStdDevs != null) {
+      m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
+    }
+  }
+
+  /**
+   * Returns the current output of the plant.
+   *
+   * @return The current output of the plant.
+   */
+  public Matrix<Outputs, N1> getOutput() {
+    return m_y;
+  }
+
+  /**
+   * Returns an element of the current output of the plant.
+   *
+   * @param row The row to return.
+   * @return An element of the current output of the plant.
+   */
+  public double getOutput(int row) {
+    return m_y.get(row, 0);
+  }
+
+  /**
+   * Sets the system inputs (usually voltages).
+   *
+   * @param u The system inputs.
+   */
+  public void setInput(Matrix<Inputs, N1> u) {
+    this.m_u = u;
+  }
+
+  /**
+   * Sets the system inputs.
+   *
+   * @param row   The row in the input matrix to set.
+   * @param value The value to set the row to.
+   */
+  public void setInput(int row, double value) {
+    m_u.set(row, 0, value);
+  }
+
+  /**
+   * Sets the system inputs.
+   *
+   * @param u An array of doubles that represent the inputs of the system.
+   */
+  public void setInput(double... u) {
+    if (u.length != m_u.getNumRows()) {
+      throw new MatrixDimensionException("Malformed input! Got " + u.length
+          + " elements instead of " + m_u.getNumRows());
+    }
+    m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u));
+  }
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The new state.
+   */
+  public void setState(Matrix<States, N1> state) {
+    m_x = state;
+  }
+
+  /**
+   * Returns the current drawn by this simulated system. Override this method to add a custom
+   * current calculation.
+   *
+   * @return The current drawn by this simulated mechanism.
+   */
+  public double getCurrentDrawAmps() {
+    return 0.0;
+  }
+
+  /**
+   * Updates the state estimate of the system.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (usually voltage).
+   * @param dtSeconds   The time difference between controller updates.
+   * @return The new state.
+   */
+  protected Matrix<States, N1> updateX(Matrix<States, N1> currentXhat,
+                                       Matrix<Inputs, N1> u, double dtSeconds) {
+    return m_plant.calculateX(currentXhat, u, dtSeconds);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java
new file mode 100644
index 0000000..7006c91
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
+import java.util.HashMap;
+import java.util.Map;
+
+public class Mechanism2D {
+  private static final SimDevice m_device = SimDevice.create("Mechanism2D");
+  private static final Map<String, SimDouble> m_createdItems = new HashMap<String, SimDouble>();
+
+  /**
+     * Set/Create the angle of a ligament.
+     *
+     * @param ligamentPath json path to the ligament
+     * @param angle        to set the ligament
+     */
+  public void setLigamentAngle(String ligamentPath, float angle) {
+    ligamentPath = ligamentPath + "/angle";
+    if (m_device != null) {
+      if (!m_createdItems.containsKey(ligamentPath)) {
+        m_createdItems.put(ligamentPath, m_device.createDouble(ligamentPath, false, angle));
+      }
+      m_createdItems.get(ligamentPath).set(angle);
+    }
+  }
+
+  /**
+   * Set/Create the length of a ligament.
+   *
+   * @param ligamentPath json path to the ligament
+   * @param length       to set the ligament
+   */
+  public void setLigamentLength(String ligamentPath, float length) {
+    ligamentPath = ligamentPath + "/length";
+    if (m_device != null) {
+      if (!m_createdItems.containsKey(ligamentPath)) {
+        m_createdItems.put(ligamentPath, m_device.createDouble(ligamentPath, false, length));
+      }
+      m_createdItems.get(ligamentPath).set(length);
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java
new file mode 100644
index 0000000..3778e36
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifierDataJNI;
+
+/**
+ * Class to control simulated notifiers.
+ */
+public final class NotifierSim {
+  private NotifierSim() {
+  }
+
+  /**
+   * Gets the timeout of the next notifier.
+   *
+   * @return Timestamp
+   */
+  public static long getNextTimeout() {
+    return NotifierDataJNI.getNextTimeout();
+  }
+
+  /**
+   * Gets the total number of notifiers.
+   *
+   * @return Count
+   */
+  public static int getNumNotifiers() {
+    return NotifierDataJNI.getNumNotifiers();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PCMSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PCMSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PCMSim.java
new file mode 100644
index 0000000..7881f0c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PCMSim.java
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.PCMDataJNI;
+import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.SensorUtil;
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+public class PCMSim {
+  private final int m_index;
+
+  /**
+   * Constructs for the default PCM.
+   */
+  public PCMSim() {
+    m_index = SensorUtil.getDefaultSolenoidModule();
+  }
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  public PCMSim(int module) {
+    m_index = module;
+  }
+
+  /**
+   * Constructs from a Compressor object.
+   *
+   * @param compressor Compressor connected to PCM to simulate
+   */
+  public PCMSim(Compressor compressor) {
+    m_index = compressor.getModule();
+  }
+
+  public CallbackStore registerSolenoidInitializedCallback(int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid = PCMDataJNI.registerSolenoidInitializedCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(m_index, channel, uid, PCMDataJNI::cancelSolenoidInitializedCallback);
+  }
+  public boolean getSolenoidInitialized(int channel) {
+    return PCMDataJNI.getSolenoidInitialized(m_index, channel);
+  }
+  public void setSolenoidInitialized(int channel, boolean solenoidInitialized) {
+    PCMDataJNI.setSolenoidInitialized(m_index, channel, solenoidInitialized);
+  }
+
+  public CallbackStore registerSolenoidOutputCallback(int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid = PCMDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(m_index, channel, uid, PCMDataJNI::cancelSolenoidOutputCallback);
+  }
+  public boolean getSolenoidOutput(int channel) {
+    return PCMDataJNI.getSolenoidOutput(m_index, channel);
+  }
+  public void setSolenoidOutput(int channel, boolean solenoidOutput) {
+    PCMDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput);
+  }
+
+  public CallbackStore registerCompressorInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PCMDataJNI.registerCompressorInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorInitializedCallback);
+  }
+  public boolean getCompressorInitialized() {
+    return PCMDataJNI.getCompressorInitialized(m_index);
+  }
+  public void setCompressorInitialized(boolean compressorInitialized) {
+    PCMDataJNI.setCompressorInitialized(m_index, compressorInitialized);
+  }
+
+  public CallbackStore registerCompressorOnCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PCMDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorOnCallback);
+  }
+  public boolean getCompressorOn() {
+    return PCMDataJNI.getCompressorOn(m_index);
+  }
+  public void setCompressorOn(boolean compressorOn) {
+    PCMDataJNI.setCompressorOn(m_index, compressorOn);
+  }
+
+  public CallbackStore registerClosedLoopEnabledCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PCMDataJNI.registerClosedLoopEnabledCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PCMDataJNI::cancelClosedLoopEnabledCallback);
+  }
+  public boolean getClosedLoopEnabled() {
+    return PCMDataJNI.getClosedLoopEnabled(m_index);
+  }
+  public void setClosedLoopEnabled(boolean closedLoopEnabled) {
+    PCMDataJNI.setClosedLoopEnabled(m_index, closedLoopEnabled);
+  }
+
+  public CallbackStore registerPressureSwitchCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PCMDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PCMDataJNI::cancelPressureSwitchCallback);
+  }
+  public boolean getPressureSwitch() {
+    return PCMDataJNI.getPressureSwitch(m_index);
+  }
+  public void setPressureSwitch(boolean pressureSwitch) {
+    PCMDataJNI.setPressureSwitch(m_index, pressureSwitch);
+  }
+
+  public CallbackStore registerCompressorCurrentCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PCMDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorCurrentCallback);
+  }
+  public double getCompressorCurrent() {
+    return PCMDataJNI.getCompressorCurrent(m_index);
+  }
+  public void setCompressorCurrent(double compressorCurrent) {
+    PCMDataJNI.setCompressorCurrent(m_index, compressorCurrent);
+  }
+
+  public void resetData() {
+    PCMDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java
new file mode 100644
index 0000000..ad57973
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.PDPDataJNI;
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
+
+/**
+ * Class to control a simulated Power Distribution Panel (PDP).
+ */
+public class PDPSim {
+  private final int m_index;
+
+  /**
+   * Constructs for the default PDP.
+   */
+  public PDPSim() {
+    m_index = 0;
+  }
+
+  /**
+   * Constructs from a PDP module number (CAN ID).
+   *
+   * @param module module number
+   */
+  public PDPSim(int module) {
+    m_index = module;
+  }
+
+  /**
+   * Constructs from a PowerDistributionPanel object.
+   *
+   * @param pdp PowerDistributionPanel to simulate
+   */
+  public PDPSim(PowerDistributionPanel pdp) {
+    m_index = pdp.getModule();
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PDPDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PDPDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return PDPDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    PDPDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerTemperatureCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PDPDataJNI.registerTemperatureCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PDPDataJNI::cancelTemperatureCallback);
+  }
+  public double getTemperature() {
+    return PDPDataJNI.getTemperature(m_index);
+  }
+  public void setTemperature(double temperature) {
+    PDPDataJNI.setTemperature(m_index, temperature);
+  }
+
+  public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PDPDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PDPDataJNI::cancelVoltageCallback);
+  }
+  public double getVoltage() {
+    return PDPDataJNI.getVoltage(m_index);
+  }
+  public void setVoltage(double voltage) {
+    PDPDataJNI.setVoltage(m_index, voltage);
+  }
+
+  public CallbackStore registerCurrentCallback(int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid = PDPDataJNI.registerCurrentCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(m_index, channel, uid, PDPDataJNI::cancelCurrentCallback);
+  }
+  public double getCurrent(int channel) {
+    return PDPDataJNI.getCurrent(m_index, channel);
+  }
+  public void setCurrent(int channel, double current) {
+    PDPDataJNI.setCurrent(m_index, channel, current);
+  }
+
+  public void resetData() {
+    PDPDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java
new file mode 100644
index 0000000..d0cca92
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.PWMDataJNI;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Class to control a simulated PWM output.
+ */
+public class PWMSim {
+  private final int m_index;
+
+  /**
+   * Constructs from a PWM object.
+   *
+   * @param pwm PWM to simulate
+   */
+  public PWMSim(PWM pwm) {
+    m_index = pwm.getChannel();
+  }
+
+  /**
+   * Constructs from a PWM channel number.
+   *
+   * @param channel Channel number
+   */
+  public PWMSim(int channel) {
+    m_index = channel;
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return PWMDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    PWMDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerRawValueCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerRawValueCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelRawValueCallback);
+  }
+  public int getRawValue() {
+    return PWMDataJNI.getRawValue(m_index);
+  }
+  public void setRawValue(int rawValue) {
+    PWMDataJNI.setRawValue(m_index, rawValue);
+  }
+
+  public CallbackStore registerSpeedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerSpeedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelSpeedCallback);
+  }
+  public double getSpeed() {
+    return PWMDataJNI.getSpeed(m_index);
+  }
+  public void setSpeed(double speed) {
+    PWMDataJNI.setSpeed(m_index, speed);
+  }
+
+  public CallbackStore registerPositionCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerPositionCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelPositionCallback);
+  }
+  public double getPosition() {
+    return PWMDataJNI.getPosition(m_index);
+  }
+  public void setPosition(double position) {
+    PWMDataJNI.setPosition(m_index, position);
+  }
+
+  public CallbackStore registerPeriodScaleCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerPeriodScaleCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelPeriodScaleCallback);
+  }
+  public int getPeriodScale() {
+    return PWMDataJNI.getPeriodScale(m_index);
+  }
+  public void setPeriodScale(int periodScale) {
+    PWMDataJNI.setPeriodScale(m_index, periodScale);
+  }
+
+  public CallbackStore registerZeroLatchCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = PWMDataJNI.registerZeroLatchCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PWMDataJNI::cancelZeroLatchCallback);
+  }
+  public boolean getZeroLatch() {
+    return PWMDataJNI.getZeroLatch(m_index);
+  }
+  public void setZeroLatch(boolean zeroLatch) {
+    PWMDataJNI.setZeroLatch(m_index, zeroLatch);
+  }
+
+  public void resetData() {
+    PWMDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java
new file mode 100644
index 0000000..dc7441e
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.RelayDataJNI;
+import edu.wpi.first.wpilibj.Relay;
+
+/**
+ * Class to control a simulated relay.
+ */
+public class RelaySim {
+  private final int m_index;
+
+  /**
+   * Constructs from a Relay object.
+   *
+   * @param relay Relay to simulate
+   */
+  public RelaySim(Relay relay) {
+    m_index = relay.getChannel();
+  }
+
+  /**
+   * Constructs from a relay channel number.
+   *
+   * @param channel Channel number
+   */
+  public RelaySim(int channel) {
+    m_index = channel;
+  }
+
+  public CallbackStore registerInitializedForwardCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = RelayDataJNI.registerInitializedForwardCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, RelayDataJNI::cancelInitializedForwardCallback);
+  }
+  public boolean getInitializedForward() {
+    return RelayDataJNI.getInitializedForward(m_index);
+  }
+  public void setInitializedForward(boolean initializedForward) {
+    RelayDataJNI.setInitializedForward(m_index, initializedForward);
+  }
+
+  public CallbackStore registerInitializedReverseCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = RelayDataJNI.registerInitializedReverseCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, RelayDataJNI::cancelInitializedReverseCallback);
+  }
+  public boolean getInitializedReverse() {
+    return RelayDataJNI.getInitializedReverse(m_index);
+  }
+  public void setInitializedReverse(boolean initializedReverse) {
+    RelayDataJNI.setInitializedReverse(m_index, initializedReverse);
+  }
+
+  public CallbackStore registerForwardCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = RelayDataJNI.registerForwardCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, RelayDataJNI::cancelForwardCallback);
+  }
+  public boolean getForward() {
+    return RelayDataJNI.getForward(m_index);
+  }
+  public void setForward(boolean forward) {
+    RelayDataJNI.setForward(m_index, forward);
+  }
+
+  public CallbackStore registerReverseCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = RelayDataJNI.registerReverseCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, RelayDataJNI::cancelReverseCallback);
+  }
+  public boolean getReverse() {
+    return RelayDataJNI.getReverse(m_index);
+  }
+  public void setReverse(boolean reverse) {
+    RelayDataJNI.setReverse(m_index, reverse);
+  }
+
+  public void resetData() {
+    RelayDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
new file mode 100644
index 0000000..f90ce0f
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
@@ -0,0 +1,201 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.RoboRioDataJNI;
+
+/**
+ * Class to control a simulated RoboRIO.
+ */
+@SuppressWarnings({"PMD.ExcessivePublicCount", "PMD.UseUtilityClass"})
+public class RoboRioSim {
+  public static CallbackStore registerFPGAButtonCallback(NotifyCallback callback,
+                                                         boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerFPGAButtonCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelFPGAButtonCallback);
+  }
+  public static boolean getFPGAButton() {
+    return RoboRioDataJNI.getFPGAButton();
+  }
+  public static void setFPGAButton(boolean fPGAButton) {
+    RoboRioDataJNI.setFPGAButton(fPGAButton);
+  }
+
+  public static CallbackStore registerVInVoltageCallback(NotifyCallback callback,
+                                                         boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerVInVoltageCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelVInVoltageCallback);
+  }
+  public static double getVInVoltage() {
+    return RoboRioDataJNI.getVInVoltage();
+  }
+  public static void setVInVoltage(double vInVoltage) {
+    RoboRioDataJNI.setVInVoltage(vInVoltage);
+  }
+
+  public static CallbackStore registerVInCurrentCallback(NotifyCallback callback,
+                                                         boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerVInCurrentCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelVInCurrentCallback);
+  }
+  public static double getVInCurrent() {
+    return RoboRioDataJNI.getVInCurrent();
+  }
+  public static void setVInCurrent(double vInCurrent) {
+    RoboRioDataJNI.setVInCurrent(vInCurrent);
+  }
+
+  public static CallbackStore registerUserVoltage6VCallback(NotifyCallback callback,
+                                                            boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserVoltage6VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserVoltage6VCallback);
+  }
+  public static double getUserVoltage6V() {
+    return RoboRioDataJNI.getUserVoltage6V();
+  }
+  public static void setUserVoltage6V(double userVoltage6V) {
+    RoboRioDataJNI.setUserVoltage6V(userVoltage6V);
+  }
+
+  public static CallbackStore registerUserCurrent6VCallback(NotifyCallback callback,
+                                                            boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserCurrent6VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserCurrent6VCallback);
+  }
+  public static double getUserCurrent6V() {
+    return RoboRioDataJNI.getUserCurrent6V();
+  }
+  public static void setUserCurrent6V(double userCurrent6V) {
+    RoboRioDataJNI.setUserCurrent6V(userCurrent6V);
+  }
+
+  public static CallbackStore registerUserActive6VCallback(NotifyCallback callback,
+                                                           boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserActive6VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserActive6VCallback);
+  }
+  public static boolean getUserActive6V() {
+    return RoboRioDataJNI.getUserActive6V();
+  }
+  public static void setUserActive6V(boolean userActive6V) {
+    RoboRioDataJNI.setUserActive6V(userActive6V);
+  }
+
+  public static CallbackStore registerUserVoltage5VCallback(NotifyCallback callback,
+                                                            boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserVoltage5VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserVoltage5VCallback);
+  }
+  public static double getUserVoltage5V() {
+    return RoboRioDataJNI.getUserVoltage5V();
+  }
+  public static void setUserVoltage5V(double userVoltage5V) {
+    RoboRioDataJNI.setUserVoltage5V(userVoltage5V);
+  }
+
+  public static CallbackStore registerUserCurrent5VCallback(NotifyCallback callback,
+                                                            boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserCurrent5VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserCurrent5VCallback);
+  }
+  public static double getUserCurrent5V() {
+    return RoboRioDataJNI.getUserCurrent5V();
+  }
+  public static void setUserCurrent5V(double userCurrent5V) {
+    RoboRioDataJNI.setUserCurrent5V(userCurrent5V);
+  }
+
+  public static CallbackStore registerUserActive5VCallback(NotifyCallback callback,
+                                                           boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserActive5VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserActive5VCallback);
+  }
+  public static boolean getUserActive5V() {
+    return RoboRioDataJNI.getUserActive5V();
+  }
+  public static void setUserActive5V(boolean userActive5V) {
+    RoboRioDataJNI.setUserActive5V(userActive5V);
+  }
+
+  public static CallbackStore registerUserVoltage3V3Callback(NotifyCallback callback,
+                                                             boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserVoltage3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserVoltage3V3Callback);
+  }
+  public static double getUserVoltage3V3() {
+    return RoboRioDataJNI.getUserVoltage3V3();
+  }
+  public static void setUserVoltage3V3(double userVoltage3V3) {
+    RoboRioDataJNI.setUserVoltage3V3(userVoltage3V3);
+  }
+
+  public static CallbackStore registerUserCurrent3V3Callback(NotifyCallback callback,
+                                                             boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserCurrent3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserCurrent3V3Callback);
+  }
+  public static double getUserCurrent3V3() {
+    return RoboRioDataJNI.getUserCurrent3V3();
+  }
+  public static void setUserCurrent3V3(double userCurrent3V3) {
+    RoboRioDataJNI.setUserCurrent3V3(userCurrent3V3);
+  }
+
+  public static CallbackStore registerUserActive3V3Callback(NotifyCallback callback,
+                                                            boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserActive3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserActive3V3Callback);
+  }
+  public static boolean getUserActive3V3() {
+    return RoboRioDataJNI.getUserActive3V3();
+  }
+  public static void setUserActive3V3(boolean userActive3V3) {
+    RoboRioDataJNI.setUserActive3V3(userActive3V3);
+  }
+
+  public static CallbackStore registerUserFaults6VCallback(NotifyCallback callback,
+                                                           boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserFaults6VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserFaults6VCallback);
+  }
+  public static int getUserFaults6V() {
+    return RoboRioDataJNI.getUserFaults6V();
+  }
+  public static void setUserFaults6V(int userFaults6V) {
+    RoboRioDataJNI.setUserFaults6V(userFaults6V);
+  }
+
+  public static CallbackStore registerUserFaults5VCallback(NotifyCallback callback,
+                                                           boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserFaults5VCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserFaults5VCallback);
+  }
+  public static int getUserFaults5V() {
+    return RoboRioDataJNI.getUserFaults5V();
+  }
+  public static void setUserFaults5V(int userFaults5V) {
+    RoboRioDataJNI.setUserFaults5V(userFaults5V);
+  }
+
+  public static CallbackStore registerUserFaults3V3Callback(NotifyCallback callback,
+                                                            boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerUserFaults3V3Callback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelUserFaults3V3Callback);
+  }
+  public static int getUserFaults3V3() {
+    return RoboRioDataJNI.getUserFaults3V3();
+  }
+  public static void setUserFaults3V3(int userFaults3V3) {
+    RoboRioDataJNI.setUserFaults3V3(userFaults3V3);
+  }
+
+  public static void resetData() {
+    RoboRioDataJNI.resetData();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java
new file mode 100644
index 0000000..ce6b5c1
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.SPIAccelerometerDataJNI;
+
+public class SPIAccelerometerSim {
+  private final int m_index;
+
+  public SPIAccelerometerSim(int index) {
+    m_index = index;
+  }
+
+  public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = SPIAccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelActiveCallback);
+  }
+  public boolean getActive() {
+    return SPIAccelerometerDataJNI.getActive(m_index);
+  }
+  public void setActive(boolean active) {
+    SPIAccelerometerDataJNI.setActive(m_index, active);
+  }
+
+  public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = SPIAccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelRangeCallback);
+  }
+  public int getRange() {
+    return SPIAccelerometerDataJNI.getRange(m_index);
+  }
+  public void setRange(int range) {
+    SPIAccelerometerDataJNI.setRange(m_index, range);
+  }
+
+  public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = SPIAccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelXCallback);
+  }
+  public double getX() {
+    return SPIAccelerometerDataJNI.getX(m_index);
+  }
+  public void setX(double x) {
+    SPIAccelerometerDataJNI.setX(m_index, x);
+  }
+
+  public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = SPIAccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelYCallback);
+  }
+  public double getY() {
+    return SPIAccelerometerDataJNI.getY(m_index);
+  }
+  public void setY(double y) {
+    SPIAccelerometerDataJNI.setY(m_index, y);
+  }
+
+  public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = SPIAccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelZCallback);
+  }
+  public double getZ() {
+    return SPIAccelerometerDataJNI.getZ(m_index);
+  }
+  public void setZ(double z) {
+    SPIAccelerometerDataJNI.setZ(m_index, z);
+  }
+
+  public void resetData() {
+    SPIAccelerometerDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
new file mode 100644
index 0000000..71c197a
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.BufferCallback;
+import edu.wpi.first.hal.simulation.ConstBufferCallback;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.SPIDataJNI;
+import edu.wpi.first.hal.simulation.SpiReadAutoReceiveBufferCallback;
+
+public class SPISim {
+  private final int m_index;
+
+  public SPISim() {
+    m_index = 0;
+  }
+
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = SPIDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, SPIDataJNI::cancelInitializedCallback);
+  }
+  public boolean getInitialized() {
+    return SPIDataJNI.getInitialized(m_index);
+  }
+  public void setInitialized(boolean initialized) {
+    SPIDataJNI.setInitialized(m_index, initialized);
+  }
+
+  public CallbackStore registerReadCallback(BufferCallback callback) {
+    int uid = SPIDataJNI.registerReadCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadCallback);
+  }
+
+  public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
+    int uid = SPIDataJNI.registerWriteCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, SPIDataJNI::cancelWriteCallback);
+  }
+
+  public CallbackStore registerReadAutoReceiveBufferCallback(SpiReadAutoReceiveBufferCallback callback) {
+    int uid = SPIDataJNI.registerReadAutoReceiveBufferCallback(m_index, callback);
+    return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadAutoReceiveBufferCallback);
+  }
+
+  public void resetData() {
+    SPIDataJNI.resetData(m_index);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java
new file mode 100644
index 0000000..713c112
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.hal.SimValue;
+import edu.wpi.first.hal.simulation.SimDeviceCallback;
+import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
+import edu.wpi.first.hal.simulation.SimValueCallback;
+
+/**
+ * Class to control the simulation side of a SimDevice.
+ */
+public class SimDeviceSim {
+  private final int m_handle;
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   */
+  public SimDeviceSim(String name) {
+    m_handle = SimDeviceDataJNI.getSimDeviceHandle(name);
+  }
+
+  public SimValue getValue(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimValue(handle);
+  }
+
+  public SimDouble getDouble(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimDouble(handle);
+  }
+
+  public SimEnum getEnum(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimEnum(handle);
+  }
+
+  public SimBoolean getBoolean(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimBoolean(handle);
+  }
+
+  public static String[] getEnumOptions(SimEnum val) {
+    return SimDeviceDataJNI.getSimValueEnumOptions(val.getNativeHandle());
+  }
+
+  public SimDeviceDataJNI.SimValueInfo[] enumerateValues() {
+    return SimDeviceDataJNI.enumerateSimValues(m_handle);
+  }
+
+  public int getNativeHandle() {
+    return m_handle;
+  }
+
+  public CallbackStore registerValueCreatedCallback(SimValueCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimValueCreatedCallback(m_handle, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueCreatedCallback);
+  }
+
+  public CallbackStore registerValueChangedCallback(SimValueCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimValueChangedCallback(m_handle, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
+  }
+
+  public static SimDeviceDataJNI.SimDeviceInfo[] enumerateDevices(String prefix) {
+    return SimDeviceDataJNI.enumerateSimDevices(prefix);
+  }
+
+  public static CallbackStore registerDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimDeviceCreatedCallback(prefix, callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceCreatedCallback);
+  }
+
+  public static CallbackStore registerDeviceFreedCallback(String prefix, SimDeviceCallback callback) {
+    int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceFreedCallback);
+  }
+
+  public static void resetData() {
+    SimDeviceDataJNI.resetSimDeviceData();
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
new file mode 100644
index 0000000..9c17e1c
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.SimulatorJNI;
+
+public final class SimHooks {
+  private SimHooks() {
+  }
+
+  public static void setHALRuntimeType(int type) {
+    SimulatorJNI.setRuntimeType(type);
+  }
+
+  public static void waitForProgramStart() {
+    SimulatorJNI.waitForProgramStart();
+  }
+
+  public static void setProgramStarted() {
+    SimulatorJNI.setProgramStarted();
+  }
+
+  public static boolean getProgramStarted() {
+    return SimulatorJNI.getProgramStarted();
+  }
+
+  public static void restartTiming() {
+    SimulatorJNI.restartTiming();
+  }
+
+  public static void pauseTiming() {
+    SimulatorJNI.pauseTiming();
+  }
+
+  public static void resumeTiming() {
+    SimulatorJNI.resumeTiming();
+  }
+
+  public static boolean isTimingPaused() {
+    return SimulatorJNI.isTimingPaused();
+  }
+
+  public static void stepTiming(double deltaSeconds) {
+    SimulatorJNI.stepTiming((long) (deltaSeconds * 1e6));
+  }
+
+  public static void stepTimingAsync(double deltaSeconds) {
+    SimulatorJNI.stepTimingAsync((long) (deltaSeconds * 1e6));
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
new file mode 100644
index 0000000..f3188a0
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
@@ -0,0 +1,250 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+/**
+ * Represents a simulated single jointed arm mechanism.
+ */
+public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
+  // The gearbox for the arm.
+  private final DCMotor m_gearbox;
+
+  // The gearing between the motors and the output.
+  private final double m_gearing;
+
+  // The length of the arm.
+  @SuppressWarnings("MemberName")
+  private final double m_r;
+
+  // The minimum angle that the arm is capable of.
+  private final double m_minAngle;
+
+  // The maximum angle that the arm is capable of.
+  private final double m_maxAngle;
+
+  // The mass of the arm.
+  private final double m_armMass;
+
+  // Whether the simulator should simulate gravity.
+  private final boolean m_simulateGravity;
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param plant           The linear system that represents the arm.
+   * @param gearbox         The type of and number of motors in the arm gearbox.
+   * @param gearing         The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param armLengthMeters The length of the arm.
+   * @param minAngleRads    The minimum angle that the arm is capable of.
+   * @param maxAngleRads    The maximum angle that the arm is capable of.
+   * @param armMassKg       The mass of the arm.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   */
+  public SingleJointedArmSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
+                             double armLengthMeters, double minAngleRads, double maxAngleRads,
+                             double armMassKg, boolean simulateGravity) {
+    this(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, armMassKg,
+        simulateGravity, null);
+  }
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param plant              The linear system that represents the arm.
+   * @param gearbox            The type of and number of motors in the arm gearbox.
+   * @param gearing            The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param armLengthMeters    The length of the arm.
+   * @param minAngleRads       The minimum angle that the arm is capable of.
+   * @param maxAngleRads       The maximum angle that the arm is capable of.
+   * @param armMassKg          The mass of the arm.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public SingleJointedArmSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
+                             double armLengthMeters, double minAngleRads, double maxAngleRads,
+                             double armMassKg, boolean simulateGravity,
+                             Matrix<N1, N1> measurementStdDevs) {
+    super(plant, measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+    m_r = armLengthMeters;
+    m_minAngle = minAngleRads;
+    m_maxAngle = maxAngleRads;
+    m_armMass = armMassKg;
+    m_simulateGravity = simulateGravity;
+  }
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox          The type of and number of motors in the arm gearbox.
+   * @param gearing          The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param jKgMetersSquared The moment of inertia of the arm. This can be calculated from CAD software.
+   * @param armLengthMeters  The length of the arm.
+   * @param minAngleRads     The minimum angle that the arm is capable of.
+   * @param maxAngleRads     The maximum angle that the arm is capable of.
+   * @param armMassKg        The mass of the arm.
+   * @param simulateGravity  Whether gravity should be simulated or not.
+   */
+  public SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
+                             double armLengthMeters, double minAngleRads, double maxAngleRads,
+                             double armMassKg, boolean simulateGravity) {
+    this(gearbox, gearing, jKgMetersSquared, armLengthMeters, minAngleRads, maxAngleRads,
+        armMassKg, simulateGravity, null);
+  }
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox            The type of and number of motors in the arm gearbox.
+   * @param gearing            The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param jKgMetersSquared   The moment of inertia of the arm. This can be calculated from CAD software.
+   * @param armLengthMeters    The length of the arm.
+   * @param minAngleRads       The minimum angle that the arm is capable of.
+   * @param maxAngleRads       The maximum angle that the arm is capable of.
+   * @param armMassKg          The mass of the arm.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
+                             double armLengthMeters, double minAngleRads, double maxAngleRads,
+                             double armMassKg, boolean simulateGravity,
+                             Matrix<N1, N1> measurementStdDevs) {
+    super(LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
+        measurementStdDevs);
+    m_gearbox = gearbox;
+    m_gearing = gearing;
+    m_r = armLengthMeters;
+    m_minAngle = minAngleRads;
+    m_maxAngle = maxAngleRads;
+    m_armMass = armMassKg;
+    m_simulateGravity = simulateGravity;
+  }
+
+  /**
+   * Returns whether the arm has hit the lower limit.
+   *
+   * @param x The current arm state.
+   * @return Whether the arm has hit the lower limit.
+   */
+  public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
+    return x.get(0, 0) < this.m_minAngle;
+  }
+
+  /**
+   * Returns whether the arm has hit the upper limit.
+   *
+   * @param x The current arm state.
+   * @return Whether the arm has hit the upper limit.
+   */
+  public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
+    return x.get(0, 0) > this.m_maxAngle;
+  }
+
+  /**
+   * Returns the current arm angle.
+   *
+   * @return The current arm angle.
+   */
+  public double getAngleRads() {
+    return m_x.get(0, 0);
+  }
+
+  /**
+   * Returns the current arm velocity.
+   *
+   * @return The current arm velocity.
+   */
+  public double getVelocityRadPerSec() {
+    return m_x.get(1, 0);
+  }
+
+  /**
+   * Returns the arm current draw.
+   *
+   * @return The aram current draw.
+   */
+  @Override
+  public double getCurrentDrawAmps() {
+    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
+    // spinning 10x faster than the output
+    var motorVelocity = m_x.get(1, 0) * m_gearing;
+    return m_gearbox.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
+  }
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param volts The input voltage.
+   */
+  public void setInputVoltage(double volts) {
+    setInput(volts);
+  }
+
+  /**
+   * Calculates a rough estimate of the moment of inertia of an arm given its
+   * length and mass.
+   *
+   * @param lengthMeters The length of the arm.
+   * @param massKg       The mass of the arm.
+   *
+   * @return The calculated moment of inertia.
+   */
+  public static double estimateMOI(double lengthMeters, double massKg) {
+    return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters;
+  }
+
+  /**
+   * Updates the state of the arm.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dtSeconds   The time difference between controller updates.
+   */
+  @Override
+  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat,
+                                   Matrix<N1, N1> u, double dtSeconds) {
+    // Horizontal case:
+    // Torque = F * r = I * alpha
+    // alpha = F * r / I
+    // Since F = mg,
+    // alpha = m * g * r / I
+    // Finally, multiply RHS by cos(theta) to account for the arm angle
+    // This acceleration is added to the linear system dynamics x-dot = Ax + Bu
+    // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
+    // cos(theta)]]
+    Matrix<N2, N1> updatedXhat = RungeKutta.rungeKutta(
+        (Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
+          Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
+          if (m_simulateGravity) {
+            xdot = xdot.plus(VecBuilder.fill(0,
+                m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * Math.cos(x.get(0, 0))));
+          }
+          return xdot;
+        },
+        currentXhat, u, dtSeconds);
+
+    // We check for collision after updating xhat
+    if (hasHitLowerLimit(updatedXhat)) {
+      return VecBuilder.fill(m_minAngle, 0);
+    }
+    if (hasHitUpperLimit(updatedXhat)) {
+      return VecBuilder.fill(m_maxAngle, 0);
+    }
+    return updatedXhat;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
new file mode 100644
index 0000000..40a02bf
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+
+/**
+ * Class to control a simulated Xbox 360 or Xbox One controller.
+ */
+public class XboxControllerSim extends GenericHIDSim {
+  /**
+   * Constructs from a XboxController object.
+   *
+   * @param joystick controller to simulate
+   */
+  public XboxControllerSim(XboxController joystick) {
+    super(joystick);
+    setAxisCount(6);
+    setButtonCount(10);
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  public XboxControllerSim(int port) {
+    super(port);
+    setAxisCount(6);
+    setButtonCount(10);
+  }
+
+  public void setX(GenericHID.Hand hand, double value) {
+    if (hand.equals(GenericHID.Hand.kLeft)) {
+      setRawAxis(XboxController.Axis.kLeftX.value, value);
+    } else {
+      setRawAxis(XboxController.Axis.kRightX.value, value);
+    }
+  }
+
+  public void setY(GenericHID.Hand hand, double value) {
+    if (hand.equals(GenericHID.Hand.kLeft)) {
+      setRawAxis(XboxController.Axis.kLeftY.value, value);
+    } else {
+      setRawAxis(XboxController.Axis.kRightY.value, value);
+    }
+  }
+
+  public void setTriggerAxis(GenericHID.Hand hand, double value) {
+    if (hand.equals(GenericHID.Hand.kLeft)) {
+      setRawAxis(XboxController.Axis.kLeftTrigger.value, value);
+    } else {
+      setRawAxis(XboxController.Axis.kRightTrigger.value, value);
+    }
+  }
+
+  public void setBumper(GenericHID.Hand hand, boolean state) {
+    if (hand.equals(GenericHID.Hand.kLeft)) {
+      setRawButton(XboxController.Button.kBumperLeft.value, state);
+    } else {
+      setRawButton(XboxController.Button.kBumperRight.value, state);
+    }
+  }
+
+  public void setStickButton(GenericHID.Hand hand, boolean state) {
+    if (hand.equals(GenericHID.Hand.kLeft)) {
+      setRawButton(XboxController.Button.kStickLeft.value, state);
+    } else {
+      setRawButton(XboxController.Button.kStickRight.value, state);
+    }
+  }
+
+  public void setAButton(boolean state) {
+    setRawButton(XboxController.Button.kA.value, state);
+  }
+
+  public void setBButton(boolean state) {
+    setRawButton(XboxController.Button.kB.value, state);
+  }
+
+  public void setXButton(boolean state) {
+    setRawButton(XboxController.Button.kX.value, state);
+  }
+
+  public void setYButton(boolean state) {
+    setRawButton(XboxController.Button.kY.value, state);
+  }
+
+  public void setBackButton(boolean state) {
+    setRawButton(XboxController.Button.kBack.value, state);
+  }
+
+  public void setStartButton(boolean state) {
+    setRawButton(XboxController.Button.kStart.value, state);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
index e0bb4f9..a82f26b 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,7 +21,6 @@
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableValue;
 
-@SuppressWarnings("PMD.TooManyMethods")
 public class SendableBuilderImpl implements SendableBuilder {
   private static class Property {
     Property(NetworkTable table, String key) {
@@ -55,7 +54,7 @@
 
   private final List<Property> m_properties = new ArrayList<>();
   private Runnable m_safeState;
-  private Runnable m_updateTable;
+  private final List<Runnable> m_updateTables = new ArrayList<>();
   private NetworkTable m_table;
   private NetworkTableEntry m_controllableEntry;
   private boolean m_actuator;
@@ -105,8 +104,8 @@
         property.m_update.accept(property.m_entry);
       }
     }
-    if (m_updateTable != null) {
-      m_updateTable.run();
+    for (Runnable updateTable : m_updateTables) {
+      updateTable.run();
     }
   }
 
@@ -207,7 +206,7 @@
    */
   @Override
   public void setUpdateTable(Runnable func) {
-    m_updateTable = func;
+    m_updateTables.add(func);
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
index a8cd557..3cafbf7 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,7 +24,6 @@
  * The SendableRegistry class is the public interface for registering sensors
  * and actuators for use on dashboards and LiveWindow.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class SendableRegistry {
   private static class Component {
     Component() {}
@@ -338,10 +337,10 @@
       return null;
     }
     Object rv = null;
-    if (handle < comp.m_data.length) {
-      rv = comp.m_data[handle];
-    } else if (comp.m_data == null) {
+    if (comp.m_data == null) {
       comp.m_data = new Object[handle + 1];
+    } else if (handle < comp.m_data.length) {
+      rv = comp.m_data[handle];
     } else {
       comp.m_data = Arrays.copyOf(comp.m_data, handle + 1);
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
index 457ae46..1b022c1 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -26,7 +26,7 @@
  * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the
  * laptop. Users can put values into and get values from the SmartDashboard.
  */
-@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods"})
+@SuppressWarnings("PMD.GodClass")
 public final class SmartDashboard {
   /**
    * The {@link NetworkTable} used by {@link SmartDashboard}.

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
deleted file mode 100644
index 578787c..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
+++ /dev/null
@@ -1,115 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import org.ejml.simple.SimpleMatrix;
-
-public class CubicHermiteSpline extends Spline {
-  private static SimpleMatrix hermiteBasis;
-  private final SimpleMatrix m_coefficients;
-
-  /**
-   * Constructs a cubic hermite spline with the specified control vectors. Each
-   * control vector contains info about the location of the point and its first
-   * derivative.
-   *
-   * @param xInitialControlVector The control vector for the initial point in
-   *                              the x dimension.
-   * @param xFinalControlVector   The control vector for the final point in
-   *                              the x dimension.
-   * @param yInitialControlVector The control vector for the initial point in
-   *                              the y dimension.
-   * @param yFinalControlVector   The control vector for the final point in
-   *                              the y dimension.
-   */
-  @SuppressWarnings("ParameterName")
-  public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
-                            double[] yInitialControlVector, double[] yFinalControlVector) {
-    super(3);
-
-    // Populate the coefficients for the actual spline equations.
-    // Row 0 is x coefficients
-    // Row 1 is y coefficients
-    final var hermite = makeHermiteBasis();
-    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-    final var xCoeffs = (hermite.mult(x)).transpose();
-    final var yCoeffs = (hermite.mult(y)).transpose();
-
-    m_coefficients = new SimpleMatrix(6, 4);
-
-    for (int i = 0; i < 4; i++) {
-      m_coefficients.set(0, i, xCoeffs.get(0, i));
-      m_coefficients.set(1, i, yCoeffs.get(0, i));
-
-      // Populate Row 2 and Row 3 with the derivatives of the equations above.
-      // Then populate row 4 and 5 with the second derivatives.
-      // Here, we are multiplying by (3 - i) to manually take the derivative. The
-      // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
-      // coefficient of the derivative, we can use the power rule and multiply
-      // the existing coefficient by its power.
-      m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
-      m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
-    }
-
-    for (int i = 0; i < 3; i++) {
-      // Here, we are multiplying by (2 - i) to manually take the derivative. The
-      // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
-      // coefficient of the derivative, we can use the power rule and multiply
-      // the existing coefficient by its power.
-      m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
-      m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
-    }
-
-  }
-
-  /**
-   * Returns the coefficients matrix.
-   *
-   * @return The coefficients matrix.
-   */
-  @Override
-  protected SimpleMatrix getCoefficients() {
-    return m_coefficients;
-  }
-
-  /**
-   * Returns the hermite basis matrix for cubic hermite spline interpolation.
-   *
-   * @return The hermite basis matrix for cubic hermite spline interpolation.
-   */
-  private SimpleMatrix makeHermiteBasis() {
-    if (hermiteBasis == null) {
-      hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{
-          +2.0, +1.0, -2.0, +1.0,
-          -3.0, -2.0, +3.0, -1.0,
-          +0.0, +1.0, +0.0, +0.0,
-          +1.0, +0.0, +0.0, +0.0
-      });
-    }
-    return hermiteBasis;
-  }
-
-  /**
-   * Returns the control vector for each dimension as a matrix from the
-   * user-provided arrays in the constructor.
-   *
-   * @param initialVector The control vector for the initial point.
-   * @param finalVector   The control vector for the final point.
-   * @return The control vector matrix for a dimension.
-   */
-  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
-    if (initialVector.length != 2 || finalVector.length != 2) {
-      throw new IllegalArgumentException("Size of vectors must be 2");
-    }
-    return new SimpleMatrix(4, 1, true, new double[]{
-        initialVector[0], initialVector[1],
-        finalVector[0], finalVector[1]});
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
deleted file mode 100644
index 62c1e5a..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-
-/**
- * Represents a pair of a pose and a curvature.
- */
-@SuppressWarnings("MemberName")
-public class PoseWithCurvature {
-  // Represents the pose.
-  public Pose2d poseMeters;
-
-  // Represents the curvature.
-  public double curvatureRadPerMeter;
-
-  /**
-   * Constructs a PoseWithCurvature.
-   *
-   * @param poseMeters           The pose.
-   * @param curvatureRadPerMeter The curvature.
-   */
-  public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
-    this.poseMeters = poseMeters;
-    this.curvatureRadPerMeter = curvatureRadPerMeter;
-  }
-
-  /**
-   * Constructs a PoseWithCurvature with default values.
-   */
-  public PoseWithCurvature() {
-    poseMeters = new Pose2d();
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
deleted file mode 100644
index 3b2d3eb..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
+++ /dev/null
@@ -1,116 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import org.ejml.simple.SimpleMatrix;
-
-public class QuinticHermiteSpline extends Spline {
-  private static SimpleMatrix hermiteBasis;
-  private final SimpleMatrix m_coefficients;
-
-  /**
-   * Constructs a quintic hermite spline with the specified control vectors.
-   * Each control vector contains into about the location of the point, its
-   * first derivative, and its second derivative.
-   *
-   * @param xInitialControlVector The control vector for the initial point in
-   *                              the x dimension.
-   * @param xFinalControlVector   The control vector for the final point in
-   *                              the x dimension.
-   * @param yInitialControlVector The control vector for the initial point in
-   *                              the y dimension.
-   * @param yFinalControlVector   The control vector for the final point in
-   *                              the y dimension.
-   */
-  @SuppressWarnings("ParameterName")
-  public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
-                              double[] yInitialControlVector, double[] yFinalControlVector) {
-    super(5);
-
-    // Populate the coefficients for the actual spline equations.
-    // Row 0 is x coefficients
-    // Row 1 is y coefficients
-    final var hermite = makeHermiteBasis();
-    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-    final var xCoeffs = (hermite.mult(x)).transpose();
-    final var yCoeffs = (hermite.mult(y)).transpose();
-
-    m_coefficients = new SimpleMatrix(6, 6);
-
-    for (int i = 0; i < 6; i++) {
-      m_coefficients.set(0, i, xCoeffs.get(0, i));
-      m_coefficients.set(1, i, yCoeffs.get(0, i));
-    }
-    for (int i = 0; i < 6; i++) {
-      // Populate Row 2 and Row 3 with the derivatives of the equations above.
-      // Here, we are multiplying by (5 - i) to manually take the derivative. The
-      // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
-      // coefficient of the derivative, we can use the power rule and multiply
-      // the existing coefficient by its power.
-      m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
-      m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
-    }
-    for (int i = 0; i < 5; i++) {
-      // Then populate row 4 and 5 with the second derivatives.
-      // Here, we are multiplying by (4 - i) to manually take the derivative. The
-      // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
-      // coefficient of the derivative, we can use the power rule and multiply
-      // the existing coefficient by its power.
-      m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
-      m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
-    }
-  }
-
-  /**
-   * Returns the coefficients matrix.
-   *
-   * @return The coefficients matrix.
-   */
-  @Override
-  protected SimpleMatrix getCoefficients() {
-    return m_coefficients;
-  }
-
-  /**
-   * Returns the hermite basis matrix for quintic hermite spline interpolation.
-   *
-   * @return The hermite basis matrix for quintic hermite spline interpolation.
-   */
-  private SimpleMatrix makeHermiteBasis() {
-    if (hermiteBasis == null) {
-      hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{
-          -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
-          +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
-          -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
-          +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
-          +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
-          +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
-      });
-    }
-    return hermiteBasis;
-  }
-
-  /**
-   * Returns the control vector for each dimension as a matrix from the
-   * user-provided arrays in the constructor.
-   *
-   * @param initialVector The control vector for the initial point.
-   * @param finalVector   The control vector for the final point.
-   * @return The control vector matrix for a dimension.
-   */
-  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
-    if (initialVector.length != 3 || finalVector.length != 3) {
-      throw new IllegalArgumentException("Size of vectors must be 3");
-    }
-    return new SimpleMatrix(6, 1, true, new double[]{
-        initialVector[0], initialVector[1], initialVector[2],
-        finalVector[0], finalVector[1], finalVector[2]});
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
deleted file mode 100644
index a0dfd19..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
+++ /dev/null
@@ -1,116 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.Arrays;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-/**
- * Represents a two-dimensional parametric spline that interpolates between two
- * points.
- */
-public abstract class Spline {
-  private final int m_degree;
-
-  /**
-   * Constructs a spline with the given degree.
-   *
-   * @param degree The degree of the spline.
-   */
-  Spline(int degree) {
-    m_degree = degree;
-  }
-
-  /**
-   * Returns the coefficients of the spline.
-   *
-   * @return The coefficients of the spline.
-   */
-  protected abstract SimpleMatrix getCoefficients();
-
-  /**
-   * Gets the pose and curvature at some point t on the spline.
-   *
-   * @param t The point t
-   * @return The pose and curvature at that point.
-   */
-  @SuppressWarnings("ParameterName")
-  public PoseWithCurvature getPoint(double t) {
-    SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
-    final var coefficients = getCoefficients();
-
-    // Populate the polynomial bases.
-    for (int i = 0; i <= m_degree; i++) {
-      polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
-    }
-
-    // This simply multiplies by the coefficients. We need to divide out t some
-    // n number of times where n is the derivative we want to take.
-    SimpleMatrix combined = coefficients.mult(polynomialBases);
-
-    // Get x and y
-    final double x = combined.get(0, 0);
-    final double y = combined.get(1, 0);
-
-    double dx;
-    double dy;
-    double ddx;
-    double ddy;
-
-    if (t == 0) {
-      dx = coefficients.get(2, m_degree - 1);
-      dy = coefficients.get(3, m_degree - 1);
-      ddx = coefficients.get(4, m_degree - 2);
-      ddy = coefficients.get(5, m_degree - 2);
-    } else {
-      // Divide out t once for first derivative.
-      dx = combined.get(2, 0) / t;
-      dy = combined.get(3, 0) / t;
-
-      // Divide out t twice for second derivative.
-      ddx = combined.get(4, 0) / t / t;
-      ddy = combined.get(5, 0) / t / t;
-    }
-
-    // Find the curvature.
-    final double curvature =
-        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
-
-    return new PoseWithCurvature(
-        new Pose2d(x, y, new Rotation2d(dx, dy)),
-        curvature
-    );
-  }
-
-  /**
-   * Represents a control vector for a spline.
-   *
-   * <p>Each element in each array represents the value of the derivative at the index. For
-   * example, the value of x[2] is the second derivative in the x dimension.
-   */
-  @SuppressWarnings("MemberName")
-  public static class ControlVector {
-    public double[] x;
-    public double[] y;
-
-    /**
-     * Instantiates a control vector.
-     * @param x The x dimension of the control vector.
-     * @param y The y dimension of the control vector.
-     */
-    @SuppressWarnings("ParameterName")
-    public ControlVector(double[] x, double[] y) {
-      this.x = Arrays.copyOf(x, x.length);
-      this.y = Arrays.copyOf(y, y.length);
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
deleted file mode 100644
index a419bf9..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
+++ /dev/null
@@ -1,276 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-public final class SplineHelper {
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private SplineHelper() {
-  }
-
-  /**
-   * Returns 2 cubic control vectors from a set of exterior waypoints and
-   * interior translations.
-   *
-   * @param start             The starting pose.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending pose.
-   * @return 2 cubic control vectors.
-   */
-  public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
-      Pose2d start, Translation2d[] interiorWaypoints, Pose2d end
-  ) {
-    // Generate control vectors from poses.
-    Spline.ControlVector initialCV;
-    Spline.ControlVector endCV;
-
-    // Chooses a magnitude automatically that makes the splines look better.
-    if (interiorWaypoints.length < 1) {
-      double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
-      initialCV = getCubicControlVector(scalar, start);
-      endCV = getCubicControlVector(scalar, end);
-    } else {
-      double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
-      initialCV = getCubicControlVector(scalar, start);
-      scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1])
-          * 1.2;
-      endCV = getCubicControlVector(scalar, end);
-    }
-    return new Spline.ControlVector[]{initialCV, endCV};
-  }
-
-  /**
-   * Returns quintic control vectors from a set of waypoints.
-   *
-   * @param waypoints The waypoints
-   * @return List of control vectors
-   */
-  public static List<Spline.ControlVector> getQuinticControlVectorsFromWaypoints(
-      List<Pose2d> waypoints
-  ) {
-    List<Spline.ControlVector> vectors = new ArrayList<>();
-    for (int i = 0; i < waypoints.size() - 1; i++) {
-      var p0 = waypoints.get(i);
-      var p1 = waypoints.get(i + 1);
-
-      // This just makes the splines look better.
-      final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
-
-      vectors.add(getQuinticControlVector(scalar, p0));
-      vectors.add(getQuinticControlVector(scalar, p1));
-    }
-    return vectors;
-  }
-
-  /**
-   * Returns a set of cubic splines corresponding to the provided control vectors. The
-   * user is free to set the direction of the start and end point. The
-   * directions for the middle waypoints are determined automatically to ensure
-   * continuous curvature throughout the path.
-   *
-   * @param start     The starting control vector.
-   * @param waypoints The middle waypoints. This can be left blank if you only
-   *                  wish to create a path with two waypoints.
-   * @param end       The ending control vector.
-   * @return A vector of cubic hermite splines that interpolate through the
-   *         provided waypoints and control vectors.
-   */
-  @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
-                        "PMD.AvoidInstantiatingObjectsInLoops"})
-  public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
-      Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
-
-    CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
-
-    double[] xInitial = start.x;
-    double[] yInitial = start.y;
-    double[] xFinal = end.x;
-    double[] yFinal = end.y;
-
-    if (waypoints.length > 1) {
-      Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
-
-      // Create an array of all waypoints, including the start and end.
-      newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
-      System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
-      newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
-
-      // Populate tridiagonal system for clamped cubic
-      /* See:
-      https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
-      /undervisningsmateriale/chap7alecture.pdf
-      */
-      // Above-diagonal of tridiagonal matrix, zero-padded
-      final double[] a = new double[newWaypts.length - 2];
-
-      // Diagonal of tridiagonal matrix
-      final double[] b = new double[newWaypts.length - 2];
-      Arrays.fill(b, 4.0);
-
-      // Below-diagonal of tridiagonal matrix, zero-padded
-      final double[] c = new double[newWaypts.length - 2];
-
-      // rhs vectors
-      final double[] dx = new double[newWaypts.length - 2];
-      final double[] dy = new double[newWaypts.length - 2];
-
-      // solution vectors
-      final double[] fx = new double[newWaypts.length - 2];
-      final double[] fy = new double[newWaypts.length - 2];
-
-      // populate above-diagonal and below-diagonal vectors
-      a[0] = 0.0;
-      for (int i = 0; i < newWaypts.length - 3; i++) {
-        a[i + 1] = 1;
-        c[i] = 1;
-      }
-      c[c.length - 1] = 0.0;
-
-      // populate rhs vectors
-      dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
-      dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
-
-      if (newWaypts.length > 4) {
-        for (int i = 1; i <= newWaypts.length - 4; i++) {
-          dx[i] = 3 * (newWaypts[i + 1].getX() - newWaypts[i - 1].getX());
-          dy[i] = 3 * (newWaypts[i + 1].getY() - newWaypts[i - 1].getY());
-        }
-      }
-
-      dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
-          - newWaypts[newWaypts.length - 3].getX()) - xFinal[1];
-      dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
-          - newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
-
-      // Compute solution to tridiagonal system
-      thomasAlgorithm(a, b, c, dx, fx);
-      thomasAlgorithm(a, b, c, dy, fy);
-
-      double[] newFx = new double[fx.length + 2];
-      double[] newFy = new double[fy.length + 2];
-
-      newFx[0] = xInitial[1];
-      newFy[0] = yInitial[1];
-      System.arraycopy(fx, 0, newFx, 1, fx.length);
-      System.arraycopy(fy, 0, newFy, 1, fy.length);
-      newFx[newFx.length - 1] = xFinal[1];
-      newFy[newFy.length - 1] = yFinal[1];
-
-      for (int i = 0; i < newFx.length - 1; i++) {
-        splines[i] = new CubicHermiteSpline(
-            new double[]{newWaypts[i].getX(), newFx[i]},
-            new double[]{newWaypts[i + 1].getX(), newFx[i + 1]},
-            new double[]{newWaypts[i].getY(), newFy[i]},
-            new double[]{newWaypts[i + 1].getY(), newFy[i + 1]}
-        );
-      }
-    } else if (waypoints.length == 1) {
-      final var xDeriv = (3 * (xFinal[0]
-          - xInitial[0])
-          - xFinal[1] - xInitial[1])
-          / 4.0;
-      final var yDeriv = (3 * (yFinal[0]
-          - yInitial[0])
-          - yFinal[1] - yInitial[1])
-          / 4.0;
-
-      double[] midXControlVector = {waypoints[0].getX(), xDeriv};
-      double[] midYControlVector = {waypoints[0].getY(), yDeriv};
-
-      splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
-                                          yInitial, midYControlVector);
-      splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
-                                          midYControlVector, yFinal);
-    } else {
-      splines[0] = new CubicHermiteSpline(xInitial, xFinal,
-                                          yInitial, yFinal);
-    }
-    return splines;
-  }
-
-  /**
-   * Returns a set of quintic splines corresponding to the provided control vectors.
-   * The user is free to set the direction of all control vectors. Continuous
-   * curvature is guaranteed throughout the path.
-   *
-   * @param controlVectors The control vectors.
-   * @return A vector of quintic hermite splines that interpolate through the
-   *         provided waypoints.
-   */
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
-      Spline.ControlVector[] controlVectors) {
-    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
-    for (int i = 0; i < controlVectors.length - 1; i++) {
-      var xInitial = controlVectors[i].x;
-      var xFinal = controlVectors[i + 1].x;
-      var yInitial = controlVectors[i].y;
-      var yFinal = controlVectors[i + 1].y;
-      splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
-                                            yInitial, yFinal);
-    }
-    return splines;
-  }
-
-  /**
-   * Thomas algorithm for solving tridiagonal systems Af = d.
-   *
-   * @param a              the values of A above the diagonal
-   * @param b              the values of A on the diagonal
-   * @param c              the values of A below the diagonal
-   * @param d              the vector on the rhs
-   * @param solutionVector the unknown (solution) vector, modified in-place
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  private static void thomasAlgorithm(double[] a, double[] b,
-                                      double[] c, double[] d, double[] solutionVector) {
-    int N = d.length;
-
-    double[] cStar = new double[N];
-    double[] dStar = new double[N];
-
-    // This updates the coefficients in the first row
-    // Note that we should be checking for division by zero here
-    cStar[0] = c[0] / b[0];
-    dStar[0] = d[0] / b[0];
-
-    // Create the c_star and d_star coefficients in the forward sweep
-    for (int i = 1; i < N; i++) {
-      double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
-      cStar[i] = c[i] * m;
-      dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
-    }
-    solutionVector[N - 1] = dStar[N - 1];
-    // This is the reverse sweep, used to update the solution vector f
-    for (int i = N - 2; i >= 0; i--) {
-      solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
-    }
-  }
-
-  private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
-    return new Spline.ControlVector(
-        new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos()},
-        new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin()}
-    );
-  }
-
-  private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
-    return new Spline.ControlVector(
-        new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos(), 0.0},
-        new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin(), 0.0}
-    );
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
deleted file mode 100644
index 4b7bf95..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
+++ /dev/null
@@ -1,152 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.ArrayDeque;
-import java.util.ArrayList;
-import java.util.List;
-
-/**
- * Class used to parameterize a spline by its arc length.
- */
-public final class SplineParameterizer {
-  private static final double kMaxDx = 0.127;
-  private static final double kMaxDy = 0.00127;
-  private static final double kMaxDtheta = 0.0872;
-
-  /**
-   * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
-   * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
-   * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
-   * this maximum should definitely indicate something has gone wrong.
-   */
-  private static final int kMaxIterations = 5000;
-
-  @SuppressWarnings("MemberName")
-  private static class StackContents {
-    final double t1;
-    final double t0;
-
-    StackContents(double t0, double t1) {
-      this.t0 = t0;
-      this.t1 = t1;
-    }
-  }
-
-  public static class MalformedSplineException extends RuntimeException {
-    /**
-     * Create a new exception with the given message.
-     *
-     * @param message the message to pass with the exception
-     */
-    private MalformedSplineException(String message) {
-      super(message);
-    }
-  }
-
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private SplineParameterizer() {
-  }
-
-  /**
-   * Parameterizes the spline. This method breaks up the spline into various
-   * arcs until their dx, dy, and dtheta are within specific tolerances.
-   *
-   * @param spline The spline to parameterize.
-   * @return A list of poses and curvatures that represents various points on the spline.
-   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
-   *                                  with approximately opposing headings)
-   */
-  public static List<PoseWithCurvature> parameterize(Spline spline) {
-    return parameterize(spline, 0.0, 1.0);
-  }
-
-  /**
-   * Parameterizes the spline. This method breaks up the spline into various
-   * arcs until their dx, dy, and dtheta are within specific tolerances.
-   *
-   * @param spline The spline to parameterize.
-   * @param t0     Starting internal spline parameter. It is recommended to use 0.0.
-   * @param t1     Ending internal spline parameter. It is recommended to use 1.0.
-   * @return       A list of poses and curvatures that represents various points on the spline.
-   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
-   *                                  with approximately opposing headings)
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
-    var splinePoints = new ArrayList<PoseWithCurvature>();
-
-    // The parameterization does not add the initial point. Let's add that.
-    splinePoints.add(spline.getPoint(t0));
-
-    // We use an "explicit stack" to simulate recursion, instead of a recursive function call
-    // This give us greater control, instead of a stack overflow
-    var stack = new ArrayDeque<StackContents>();
-    stack.push(new StackContents(t0, t1));
-
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
-    int iterations = 0;
-
-    while (!stack.isEmpty()) {
-      current = stack.removeFirst();
-      start = spline.getPoint(current.t0);
-      end = spline.getPoint(current.t1);
-
-      final var twist = start.poseMeters.log(end.poseMeters);
-      if (
-          Math.abs(twist.dy) > kMaxDy
-          || Math.abs(twist.dx) > kMaxDx
-          || Math.abs(twist.dtheta) > kMaxDtheta
-      ) {
-        stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
-        stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
-      } else {
-        splinePoints.add(spline.getPoint(current.t1));
-      }
-
-      iterations++;
-      if (iterations >= kMaxIterations) {
-        throw new MalformedSplineException(
-          "Could not parameterize a malformed spline. "
-          + "This means that you probably had two or more adjacent waypoints that were very close "
-          + "together with headings in opposing directions."
-        );
-      }
-    }
-
-    return splinePoints;
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
deleted file mode 100644
index f1d51b9..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
+++ /dev/null
@@ -1,331 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Objects;
-import java.util.stream.Collectors;
-
-import com.fasterxml.jackson.annotation.JsonProperty;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-
-/**
- * Represents a time-parameterized trajectory. The trajectory contains of
- * various States that represent the pose, curvature, time elapsed, velocity,
- * and acceleration at that point.
- */
-public class Trajectory {
-  private final double m_totalTimeSeconds;
-  private final List<State> m_states;
-
-  /**
-   * Constructs a trajectory from a vector of states.
-   *
-   * @param states A vector of states.
-   */
-  public Trajectory(final List<State> states) {
-    m_states = states;
-    m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
-  }
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("ParameterName")
-  private static double lerp(double startValue, double endValue, double t) {
-    return startValue + (endValue - startValue) * t;
-  }
-
-  /**
-   * Linearly interpolates between two poses.
-   *
-   * @param startValue The start pose.
-   * @param endValue   The end pose.
-   * @param t          The fraction for interpolation.
-   * @return The interpolated pose.
-   */
-  @SuppressWarnings("ParameterName")
-  private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
-    return startValue.plus((endValue.minus(startValue)).times(t));
-  }
-
-  /**
-   * Returns the initial pose of the trajectory.
-   *
-   * @return The initial pose of the trajectory.
-   */
-  public Pose2d getInitialPose() {
-    return sample(0).poseMeters;
-  }
-
-  /**
-   * Returns the overall duration of the trajectory.
-   *
-   * @return The duration of the trajectory.
-   */
-  public double getTotalTimeSeconds() {
-    return m_totalTimeSeconds;
-  }
-
-  /**
-   * Return the states of the trajectory.
-   *
-   * @return The states of the trajectory.
-   */
-  public List<State> getStates() {
-    return m_states;
-  }
-
-  /**
-   * Sample the trajectory at a point in time.
-   *
-   * @param timeSeconds The point in time since the beginning of the trajectory to sample.
-   * @return The state at that point in time.
-   */
-  public State sample(double timeSeconds) {
-    if (timeSeconds <= m_states.get(0).timeSeconds) {
-      return m_states.get(0);
-    }
-    if (timeSeconds >= m_totalTimeSeconds) {
-      return m_states.get(m_states.size() - 1);
-    }
-
-    // To get the element that we want, we will use a binary search algorithm
-    // instead of iterating over a for-loop. A binary search is O(std::log(n))
-    // whereas searching using a loop is O(n).
-
-    // This starts at 1 because we use the previous state later on for
-    // interpolation.
-    int low = 1;
-    int high = m_states.size() - 1;
-
-    while (low != high) {
-      int mid = (low + high) / 2;
-      if (m_states.get(mid).timeSeconds < timeSeconds) {
-        // This index and everything under it are less than the requested
-        // timestamp. Therefore, we can discard them.
-        low = mid + 1;
-      } else {
-        // t is at least as large as the element at this index. This means that
-        // anything after it cannot be what we are looking for.
-        high = mid;
-      }
-    }
-
-    // High and Low should be the same.
-
-    // The sample's timestamp is now greater than or equal to the requested
-    // timestamp. If it is greater, we need to interpolate between the
-    // previous state and the current state to get the exact state that we
-    // want.
-    final State sample = m_states.get(low);
-    final State prevSample = m_states.get(low - 1);
-
-    // If the difference in states is negligible, then we are spot on!
-    if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
-      return sample;
-    }
-    // Interpolate between the two states for the state that we want.
-    return prevSample.interpolate(sample,
-        (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
-  }
-
-  /**
-   * Transforms all poses in the trajectory by the given transform. This is
-   * useful for converting a robot-relative trajectory into a field-relative
-   * trajectory. This works with respect to the first pose in the trajectory.
-   *
-   * @param transform The transform to transform the trajectory by.
-   * @return The transformed trajectory.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public Trajectory transformBy(Transform2d transform) {
-    var firstState = m_states.get(0);
-    var firstPose = firstState.poseMeters;
-
-    // Calculate the transformed first pose.
-    var newFirstPose = firstPose.plus(transform);
-    List<State> newStates = new ArrayList<>();
-
-    newStates.add(new State(
-        firstState.timeSeconds, firstState.velocityMetersPerSecond,
-        firstState.accelerationMetersPerSecondSq, newFirstPose, firstState.curvatureRadPerMeter
-    ));
-
-    for (int i = 1; i < m_states.size(); i++) {
-      var state = m_states.get(i);
-      // We are transforming relative to the coordinate frame of the new initial pose.
-      newStates.add(new State(
-          state.timeSeconds, state.velocityMetersPerSecond,
-          state.accelerationMetersPerSecondSq, newFirstPose.plus(state.poseMeters.minus(firstPose)),
-          state.curvatureRadPerMeter
-      ));
-    }
-
-    return new Trajectory(newStates);
-  }
-
-  /**
-   * Transforms all poses in the trajectory so that they are relative to the
-   * given pose. This is useful for converting a field-relative trajectory
-   * into a robot-relative trajectory.
-   *
-   * @param pose The pose that is the origin of the coordinate frame that
-   *             the current trajectory will be transformed into.
-   * @return The transformed trajectory.
-   */
-  public Trajectory relativeTo(Pose2d pose) {
-    return new Trajectory(m_states.stream().map(state -> new State(state.timeSeconds,
-        state.velocityMetersPerSecond, state.accelerationMetersPerSecondSq,
-        state.poseMeters.relativeTo(pose), state.curvatureRadPerMeter))
-        .collect(Collectors.toList()));
-  }
-
-  /**
-   * Represents a time-parameterized trajectory. The trajectory contains of
-   * various States that represent the pose, curvature, time elapsed, velocity,
-   * and acceleration at that point.
-   */
-  @SuppressWarnings("MemberName")
-  public static class State {
-    // The time elapsed since the beginning of the trajectory.
-    @JsonProperty("time")
-    public double timeSeconds;
-
-    // The speed at that point of the trajectory.
-    @JsonProperty("velocity")
-    public double velocityMetersPerSecond;
-
-    // The acceleration at that point of the trajectory.
-    @JsonProperty("acceleration")
-    public double accelerationMetersPerSecondSq;
-
-    // The pose at that point of the trajectory.
-    @JsonProperty("pose")
-    public Pose2d poseMeters;
-
-    // The curvature at that point of the trajectory.
-    @JsonProperty("curvature")
-    public double curvatureRadPerMeter;
-
-    public State() {
-      poseMeters = new Pose2d();
-    }
-
-    /**
-     * Constructs a State with the specified parameters.
-     *
-     * @param timeSeconds                   The time elapsed since the beginning of the trajectory.
-     * @param velocityMetersPerSecond       The speed at that point of the trajectory.
-     * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
-     * @param poseMeters                    The pose at that point of the trajectory.
-     * @param curvatureRadPerMeter          The curvature at that point of the trajectory.
-     */
-    public State(double timeSeconds, double velocityMetersPerSecond,
-                 double accelerationMetersPerSecondSq, Pose2d poseMeters,
-                 double curvatureRadPerMeter) {
-      this.timeSeconds = timeSeconds;
-      this.velocityMetersPerSecond = velocityMetersPerSecond;
-      this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
-      this.poseMeters = poseMeters;
-      this.curvatureRadPerMeter = curvatureRadPerMeter;
-    }
-
-    /**
-     * Interpolates between two States.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i        The interpolant (fraction).
-     * @return The interpolated state.
-     */
-    @SuppressWarnings("ParameterName")
-    State interpolate(State endValue, double i) {
-      // Find the new t value.
-      final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
-
-      // Find the delta time between the current state and the interpolated state.
-      final double deltaT = newT - timeSeconds;
-
-      // If delta time is negative, flip the order of interpolation.
-      if (deltaT < 0) {
-        return endValue.interpolate(this, 1 - i);
-      }
-
-      // Check whether the robot is reversing at this stage.
-      final boolean reversing = velocityMetersPerSecond < 0
-          || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
-
-      // Calculate the new velocity
-      // v_f = v_0 + at
-      final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
-
-      // Calculate the change in position.
-      // delta_s = v_0 t + 0.5 at^2
-      final double newS = (velocityMetersPerSecond * deltaT
-          + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0);
-
-      // Return the new state. To find the new position for the new state, we need
-      // to interpolate between the two endpoint poses. The fraction for
-      // interpolation is the change in position (delta s) divided by the total
-      // distance between the two endpoints.
-      final double interpolationFrac = newS
-          / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
-
-      return new State(
-          newT, newV, accelerationMetersPerSecondSq,
-          lerp(poseMeters, endValue.poseMeters, interpolationFrac),
-          lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
-      );
-    }
-
-    @Override
-    public String toString() {
-      return String.format(
-        "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
-        timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
-        poseMeters, curvatureRadPerMeter);
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof State)) {
-        return false;
-      }
-      State state = (State) obj;
-      return Double.compare(state.timeSeconds, timeSeconds) == 0
-              && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
-              && Double.compare(state.accelerationMetersPerSecondSq,
-                accelerationMetersPerSecondSq) == 0
-              && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
-              && Objects.equals(poseMeters, state.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(timeSeconds, velocityMetersPerSecond,
-              accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
-    }
-  }
-
-  @Override
-  public String toString() {
-    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
-    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
deleted file mode 100644
index fbc5ddb..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
+++ /dev/null
@@ -1,193 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
-
-/**
- * Represents the configuration for generating a trajectory. This class stores the start velocity,
- * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
- *
- * <p>The class must be constructed with a max velocity and max acceleration. The other parameters
- * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable
- * values (0, 0, {}, false). These values can be changed via the setXXX methods.
- */
-public class TrajectoryConfig {
-  private final double m_maxVelocity;
-  private final double m_maxAcceleration;
-  private final List<TrajectoryConstraint> m_constraints;
-  private double m_startVelocity;
-  private double m_endVelocity;
-  private boolean m_reversed;
-
-  /**
-   * Constructs the trajectory configuration class.
-   *
-   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
-   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
-   */
-  public TrajectoryConfig(double maxVelocityMetersPerSecond,
-                          double maxAccelerationMetersPerSecondSq) {
-    m_maxVelocity = maxVelocityMetersPerSecond;
-    m_maxAcceleration = maxAccelerationMetersPerSecondSq;
-    m_constraints = new ArrayList<>();
-  }
-
-  /**
-   * Adds a user-defined constraint to the trajectory.
-   *
-   * @param constraint The user-defined constraint.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
-    m_constraints.add(constraint);
-    return this;
-  }
-
-  /**
-   * Adds all user-defined constraints from a list to the trajectory.
-   * @param constraints List of user-defined constraints.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
-    m_constraints.addAll(constraints);
-    return this;
-  }
-
-  /**
-   * Adds a differential drive kinematics constraint to ensure that
-   * no wheel velocity of a differential drive goes above the max velocity.
-   *
-   * @param kinematics The differential drive kinematics.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
-    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
-    return this;
-  }
-
-  /**
-  * Adds a mecanum drive kinematics constraint to ensure that
-  * no wheel velocity of a mecanum drive goes above the max velocity.
-  *
-  * @param kinematics The mecanum drive kinematics.
-  * @return Instance of the current config object.
-  */
-  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
-    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
-    return this;
-  }
-
-  /**
-  * Adds a swerve drive kinematics constraint to ensure that
-  * no wheel velocity of a swerve drive goes above the max velocity.
-  *
-  * @param kinematics The swerve drive kinematics.
-  * @return Instance of the current config object.
-  */
-  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
-    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
-    return this;
-  }
-
-  /**
-  * Returns the starting velocity of the trajectory.
-  *
-  * @return The starting velocity of the trajectory.
-  */
-  public double getStartVelocity() {
-    return m_startVelocity;
-  }
-
-  /**
-   * Sets the start velocity of the trajectory.
-   *
-   * @param startVelocityMetersPerSecond The start velocity of the trajectory.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
-    m_startVelocity = startVelocityMetersPerSecond;
-    return this;
-  }
-
-  /**
-   * Returns the starting velocity of the trajectory.
-   *
-   * @return The starting velocity of the trajectory.
-   */
-  public double getEndVelocity() {
-    return m_endVelocity;
-  }
-
-  /**
-   * Sets the end velocity of the trajectory.
-   *
-   * @param endVelocityMetersPerSecond The end velocity of the trajectory.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
-    m_endVelocity = endVelocityMetersPerSecond;
-    return this;
-  }
-
-  /**
-   * Returns the maximum velocity of the trajectory.
-   *
-   * @return The maximum velocity of the trajectory.
-   */
-  public double getMaxVelocity() {
-    return m_maxVelocity;
-  }
-
-  /**
-   * Returns the maximum acceleration of the trajectory.
-   *
-   * @return The maximum acceleration of the trajectory.
-   */
-  public double getMaxAcceleration() {
-    return m_maxAcceleration;
-  }
-
-  /**
-   * Returns the user-defined constraints of the trajectory.
-   *
-   * @return The user-defined constraints of the trajectory.
-   */
-  public List<TrajectoryConstraint> getConstraints() {
-    return m_constraints;
-  }
-
-  /**
-   * Returns whether the trajectory is reversed or not.
-   *
-   * @return whether the trajectory is reversed or not.
-   */
-  public boolean isReversed() {
-    return m_reversed;
-  }
-
-  /**
-   * Sets the reversed flag of the trajectory.
-   *
-   * @param reversed Whether the trajectory should be reversed or not.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setReversed(boolean reversed) {
-    m_reversed = reversed;
-    return this;
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
deleted file mode 100644
index 2e2ae7d..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
+++ /dev/null
@@ -1,229 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collection;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
-import edu.wpi.first.wpilibj.spline.Spline;
-import edu.wpi.first.wpilibj.spline.SplineHelper;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
-
-public final class TrajectoryGenerator {
-  private static final Trajectory kDoNothingTrajectory =
-      new Trajectory(Arrays.asList(new Trajectory.State()));
-
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private TrajectoryGenerator() {
-  }
-
-  /**
-   * Generates a trajectory from the given control vectors and config. This method uses clamped
-   * cubic splines -- a method in which the exterior control vectors and interior waypoints
-   * are provided. The headings are automatically determined at the interior points to
-   * ensure continuous curvature.
-   *
-   * @param initial           The initial control vector.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending control vector.
-   * @param config            The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  public static Trajectory generateTrajectory(
-      Spline.ControlVector initial,
-      List<Translation2d> interiorWaypoints,
-      Spline.ControlVector end,
-      TrajectoryConfig config
-  ) {
-    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
-
-    // Clone the control vectors.
-    var newInitial = new Spline.ControlVector(initial.x, initial.y);
-    var newEnd = new Spline.ControlVector(end.x, end.y);
-
-    // Change the orientation if reversed.
-    if (config.isReversed()) {
-      newInitial.x[1] *= -1;
-      newInitial.y[1] *= -1;
-      newEnd.x[1] *= -1;
-      newEnd.y[1] *= -1;
-    }
-
-    // Get the spline points
-    List<PoseWithCurvature> points;
-    try {
-      points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
-          interiorWaypoints.toArray(new Translation2d[0]), newEnd));
-    } catch (MalformedSplineException ex) {
-      DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
-      return kDoNothingTrajectory;
-    }
-
-    // Change the points back to their original orientation.
-    if (config.isReversed()) {
-      for (var point : points) {
-        point.poseMeters = point.poseMeters.plus(flip);
-        point.curvatureRadPerMeter *= -1;
-      }
-    }
-
-    // Generate and return trajectory.
-    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
-        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
-        config.getMaxAcceleration(), config.isReversed());
-  }
-
-  /**
-   * Generates a trajectory from the given waypoints and config. This method uses clamped
-   * cubic splines -- a method in which the initial pose, final pose, and interior waypoints
-   * are provided.  The headings are automatically determined at the interior points to
-   * ensure continuous curvature.
-   *
-   * @param start             The starting pose.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending pose.
-   * @param config            The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  public static Trajectory generateTrajectory(
-      Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end,
-      TrajectoryConfig config
-  ) {
-    var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(
-        start, interiorWaypoints.toArray(new Translation2d[0]), end
-    );
-
-    // Return the generated trajectory.
-    return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
-  }
-
-  /**
-   * Generates a trajectory from the given quintic control vectors and config. This method
-   * uses quintic hermite splines -- therefore, all points must be represented by control
-   * vectors. Continuous curvature is guaranteed in this method.
-   *
-   * @param controlVectors List of quintic control vectors.
-   * @param config         The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public static Trajectory generateTrajectory(
-      ControlVectorList controlVectors,
-      TrajectoryConfig config
-  ) {
-    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
-    final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
-
-    // Create a new control vector list, flipping the orientation if reversed.
-    for (final var vector : controlVectors) {
-      var newVector = new Spline.ControlVector(vector.x, vector.y);
-      if (config.isReversed()) {
-        newVector.x[1] *= -1;
-        newVector.y[1] *= -1;
-      }
-      newControlVectors.add(newVector);
-    }
-
-    // Get the spline points
-    List<PoseWithCurvature> points;
-    try {
-      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
-          newControlVectors.toArray(new Spline.ControlVector[]{})
-      ));
-    } catch (MalformedSplineException ex) {
-      DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
-      return kDoNothingTrajectory;
-    }
-
-    // Change the points back to their original orientation.
-    if (config.isReversed()) {
-      for (var point : points) {
-        point.poseMeters = point.poseMeters.plus(flip);
-        point.curvatureRadPerMeter *= -1;
-      }
-    }
-
-    // Generate and return trajectory.
-    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
-        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
-        config.getMaxAcceleration(), config.isReversed());
-
-  }
-
-  /**
-   * Generates a trajectory from the given waypoints and config. This method
-   * uses quintic hermite splines -- therefore, all points must be represented by Pose2d
-   * objects. Continuous curvature is guaranteed in this method.
-   *
-   * @param waypoints List of waypoints..
-   * @param config    The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
-    var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints);
-    var newList = new ControlVectorList(originalList);
-    return generateTrajectory(newList, config);
-  }
-
-  /**
-   * Generate spline points from a vector of splines by parameterizing the
-   * splines.
-   *
-   * @param splines The splines to parameterize.
-   * @return The spline points for use in time parameterization of a trajectory.
-   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
-   *                                  with approximately opposing headings)
-   */
-  public static List<PoseWithCurvature> splinePointsFromSplines(
-      Spline[] splines) {
-    // Create the vector of spline points.
-    var splinePoints = new ArrayList<PoseWithCurvature>();
-
-    // Add the first point to the vector.
-    splinePoints.add(splines[0].getPoint(0.0));
-
-    // Iterate through the vector and parameterize each spline, adding the
-    // parameterized points to the final vector.
-    for (final var spline : splines) {
-      var points = SplineParameterizer.parameterize(spline);
-
-      // Append the array of poses to the vector. We are removing the first
-      // point because it's a duplicate of the last point from the previous
-      // spline.
-      splinePoints.addAll(points.subList(1, points.size()));
-    }
-    return splinePoints;
-  }
-
-  // Work around type erasure signatures
-  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
-    public ControlVectorList(int initialCapacity) {
-      super(initialCapacity);
-    }
-
-    public ControlVectorList() {
-      super();
-    }
-
-    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
-      super(collection);
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
deleted file mode 100644
index 9459dd0..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
+++ /dev/null
@@ -1,303 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
-import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
-
-/**
- * Class used to parameterize a trajectory by time.
- */
-public final class TrajectoryParameterizer {
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private TrajectoryParameterizer() {
-  }
-
-  /**
-   * Parameterize the trajectory by time. This is where the velocity profile is
-   * generated.
-   *
-   * <p>The derivation of the algorithm used can be found
-   * <a href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">
-   * here</a>.
-   *
-   * @param points                           Reference to the spline points.
-   * @param constraints                      A vector of various velocity and acceleration.
-   *                                         constraints.
-   * @param startVelocityMetersPerSecond     The start velocity for the trajectory.
-   * @param endVelocityMetersPerSecond       The end velocity for the trajectory.
-   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
-   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
-   * @param reversed                         Whether the robot should move backwards.
-   *                                         Note that the robot will still move from
-   *                                         a -&gt; b -&gt; ... -&gt; z as defined in the
-   *                                         waypoints.
-   * @return The trajectory.
-   */
-  @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
-      "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
-      "PMD.AvoidThrowingRawExceptionTypes"})
-  public static Trajectory timeParameterizeTrajectory(
-      List<PoseWithCurvature> points,
-      List<TrajectoryConstraint> constraints,
-      double startVelocityMetersPerSecond,
-      double endVelocityMetersPerSecond,
-      double maxVelocityMetersPerSecond,
-      double maxAccelerationMetersPerSecondSq,
-      boolean reversed
-  ) {
-    var constrainedStates = new ArrayList<ConstrainedState>(points.size());
-    var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond,
-        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
-
-    // Forward pass
-    for (int i = 0; i < points.size(); i++) {
-      constrainedStates.add(new ConstrainedState());
-      var constrainedState = constrainedStates.get(i);
-      constrainedState.pose = points.get(i);
-
-      // Begin constraining based on predecessor.
-      double ds = constrainedState.pose.poseMeters.getTranslation().getDistance(
-          predecessor.pose.poseMeters.getTranslation());
-      constrainedState.distanceMeters = predecessor.distanceMeters + ds;
-
-      // We may need to iterate to find the maximum end velocity and common
-      // acceleration, since acceleration limits may be a function of velocity.
-      while (true) {
-        // Enforce global max velocity and max reachable velocity by global
-        // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
-        constrainedState.maxVelocityMetersPerSecond = Math.min(
-            maxVelocityMetersPerSecond,
-            Math.sqrt(predecessor.maxVelocityMetersPerSecond
-                * predecessor.maxVelocityMetersPerSecond
-                + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)
-        );
-
-        constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
-        constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
-
-        // At this point, the constrained state is fully constructed apart from
-        // all the custom-defined user constraints.
-        for (final var constraint : constraints) {
-          constrainedState.maxVelocityMetersPerSecond = Math.min(
-              constrainedState.maxVelocityMetersPerSecond,
-              constraint.getMaxVelocityMetersPerSecond(
-                  constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter,
-                  constrainedState.maxVelocityMetersPerSecond)
-          );
-        }
-
-        // Now enforce all acceleration limits.
-        enforceAccelerationLimits(reversed, constraints, constrainedState);
-
-        if (ds < 1E-6) {
-          break;
-        }
-
-        // If the actual acceleration for this state is higher than the max
-        // acceleration that we applied, then we need to reduce the max
-        // acceleration of the predecessor and try again.
-        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
-            * constrainedState.maxVelocityMetersPerSecond
-            - predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond)
-            / (ds * 2.0);
-
-        // If we violate the max acceleration constraint, let's modify the
-        // predecessor.
-        if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
-          predecessor.maxAccelerationMetersPerSecondSq
-              = constrainedState.maxAccelerationMetersPerSecondSq;
-        } else {
-          // Constrain the predecessor's max acceleration to the current
-          // acceleration.
-          if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
-            predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
-          }
-          // If the actual acceleration is less than the predecessor's min
-          // acceleration, it will be repaired in the backward pass.
-          break;
-        }
-      }
-      predecessor = constrainedState;
-    }
-
-    var successor = new ConstrainedState(points.get(points.size() - 1),
-        constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
-        endVelocityMetersPerSecond,
-        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
-
-    // Backward pass
-    for (int i = points.size() - 1; i >= 0; i--) {
-      var constrainedState = constrainedStates.get(i);
-      double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
-
-      while (true) {
-        // Enforce max velocity limit (reverse)
-        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
-        double newMaxVelocity = Math.sqrt(
-            successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
-                + successor.minAccelerationMetersPerSecondSq * ds * 2.0
-        );
-
-        // No more limits to impose! This state can be finalized.
-        if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
-          break;
-        }
-
-        constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
-
-        // Check all acceleration constraints with the new max velocity.
-        enforceAccelerationLimits(reversed, constraints, constrainedState);
-
-        if (ds > -1E-6) {
-          break;
-        }
-
-        // If the actual acceleration for this state is lower than the min
-        // acceleration, then we need to lower the min acceleration of the
-        // successor and try again.
-        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
-            * constrainedState.maxVelocityMetersPerSecond
-            - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
-            / (ds * 2.0);
-
-        if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
-          successor.minAccelerationMetersPerSecondSq
-              = constrainedState.minAccelerationMetersPerSecondSq;
-        } else {
-          successor.minAccelerationMetersPerSecondSq = actualAcceleration;
-          break;
-        }
-      }
-      successor = constrainedState;
-    }
-
-    // Now we can integrate the constrained states forward in time to obtain our
-    // trajectory states.
-    var states = new ArrayList<Trajectory.State>(points.size());
-    double timeSeconds = 0.0;
-    double distanceMeters = 0.0;
-    double velocityMetersPerSecond = 0.0;
-
-    for (int i = 0; i < constrainedStates.size(); i++) {
-      final var state = constrainedStates.get(i);
-
-      // Calculate the change in position between the current state and the previous
-      // state.
-      double ds = state.distanceMeters - distanceMeters;
-
-      // Calculate the acceleration between the current state and the previous
-      // state.
-      double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
-          - velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2);
-
-      // Calculate dt
-      double dt = 0.0;
-      if (i > 0) {
-        states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
-        if (Math.abs(accel) > 1E-6) {
-          // v_f = v_0 + a * t
-          dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
-        } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
-          // delta_x = v * t
-          dt = ds / velocityMetersPerSecond;
-        } else {
-          throw new RuntimeException("Something went wrong");
-        }
-      }
-
-      velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
-      distanceMeters = state.distanceMeters;
-
-      timeSeconds += dt;
-
-      states.add(new Trajectory.State(
-          timeSeconds,
-          reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
-          reversed ? -accel : accel,
-          state.pose.poseMeters, state.pose.curvatureRadPerMeter
-      ));
-    }
-
-    return new Trajectory(states);
-  }
-
-  private static void enforceAccelerationLimits(boolean reverse,
-                                                List<TrajectoryConstraint> constraints,
-                                                ConstrainedState state) {
-
-    for (final var constraint : constraints) {
-      double factor = reverse ? -1.0 : 1.0;
-      final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq(
-          state.pose.poseMeters, state.pose.curvatureRadPerMeter,
-          state.maxVelocityMetersPerSecond * factor);
-
-      state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
-          reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
-              : minMaxAccel.minAccelerationMetersPerSecondSq);
-
-      state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq,
-          reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq
-              : minMaxAccel.maxAccelerationMetersPerSecondSq);
-    }
-
-  }
-
-  @SuppressWarnings("MemberName")
-  private static class ConstrainedState {
-    PoseWithCurvature pose;
-    double distanceMeters;
-    double maxVelocityMetersPerSecond;
-    double minAccelerationMetersPerSecondSq;
-    double maxAccelerationMetersPerSecondSq;
-
-    ConstrainedState(PoseWithCurvature pose, double distanceMeters,
-                     double maxVelocityMetersPerSecond,
-                     double minAccelerationMetersPerSecondSq,
-                     double maxAccelerationMetersPerSecondSq) {
-      this.pose = pose;
-      this.distanceMeters = distanceMeters;
-      this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
-      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
-      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
-    }
-
-    ConstrainedState() {
-      pose = new PoseWithCurvature();
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
deleted file mode 100644
index 0b24411..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
+++ /dev/null
@@ -1,304 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.Objects;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-
-/**
- * A trapezoid-shaped velocity profile.
- *
- * <p>While this class can be used for a profiled movement from start to finish,
- * the intended usage is to filter a reference's dynamics based on trapezoidal
- * velocity constraints. To compute the reference obeying this constraint, do
- * the following.
- *
- * <p>Initialization:
- * <pre><code>
- * TrapezoidProfile.Constraints constraints =
- *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
- * TrapezoidProfile.State previousProfiledReference =
- *   new TrapezoidProfile.State(initialReference, 0.0);
- * </code></pre>
- *
- * <p>Run on update:
- * <pre><code>
- * TrapezoidProfile profile =
- *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
- * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
- * </code></pre>
- *
- * <p>where `unprofiledReference` is free to change between calls. Note that when
- * the unprofiled reference is within the constraints, `calculate()` returns the
- * unprofiled reference unchanged.
- *
- * <p>Otherwise, a timer can be started to provide monotonic values for
- * `calculate()` and to determine when the profile has completed via
- * `isFinished()`.
- */
-public class TrapezoidProfile {
-  // The direction of the profile, either 1 for forwards or -1 for inverted
-  private int m_direction;
-
-  private Constraints m_constraints;
-  private State m_initial;
-  private State m_goal;
-
-  private double m_endAccel;
-  private double m_endFullSpeed;
-  private double m_endDeccel;
-
-  public static class Constraints {
-    @SuppressWarnings("MemberName")
-    public double maxVelocity;
-    @SuppressWarnings("MemberName")
-    public double maxAcceleration;
-
-    public Constraints() {
-      HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
-    }
-
-    /**
-     * Construct constraints for a TrapezoidProfile.
-     *
-     * @param maxVelocity maximum velocity
-     * @param maxAcceleration maximum acceleration
-     */
-    public Constraints(double maxVelocity, double maxAcceleration) {
-      this.maxVelocity = maxVelocity;
-      this.maxAcceleration = maxAcceleration;
-      HAL.report(tResourceType.kResourceType_TrapezoidProfile, 1);
-    }
-  }
-
-  public static class State {
-    @SuppressWarnings("MemberName")
-    public double position;
-    @SuppressWarnings("MemberName")
-    public double velocity;
-
-    public State() {
-    }
-
-    public State(double position, double velocity) {
-      this.position = position;
-      this.velocity = velocity;
-    }
-
-    @Override
-    public boolean equals(Object other) {
-      if (other instanceof State) {
-        State rhs = (State) other;
-        return this.position == rhs.position && this.velocity == rhs.velocity;
-      } else {
-        return false;
-      }
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(position, velocity);
-    }
-  }
-
-  /**
-   * Construct a TrapezoidProfile.
-   *
-   * @param constraints The constraints on the profile, like maximum velocity.
-   * @param goal        The desired state when the profile is complete.
-   * @param initial     The initial state (usually the current state).
-   */
-  public TrapezoidProfile(Constraints constraints, State goal, State initial) {
-    m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
-    m_constraints = constraints;
-    m_initial = direct(initial);
-    m_goal = direct(goal);
-
-    if (m_initial.velocity > m_constraints.maxVelocity) {
-      m_initial.velocity = m_constraints.maxVelocity;
-    }
-
-    // Deal with a possibly truncated motion profile (with nonzero initial or
-    // final velocity) by calculating the parameters as if the profile began and
-    // ended at zero velocity
-    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
-    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
-
-    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
-    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
-
-    // Now we can calculate the parameters as if it was a full trapezoid instead
-    // of a truncated one
-
-    double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
-        + cutoffDistEnd;
-    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
-
-    double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
-        * m_constraints.maxAcceleration;
-
-    // Handle the case where the profile never reaches full speed
-    if (fullSpeedDist < 0) {
-      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
-      fullSpeedDist = 0;
-    }
-
-    m_endAccel = accelerationTime - cutoffBegin;
-    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
-    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
-  }
-
-  /**
-   * Construct a TrapezoidProfile.
-   *
-   * @param constraints The constraints on the profile, like maximum velocity.
-   * @param goal        The desired state when the profile is complete.
-   */
-  public TrapezoidProfile(Constraints constraints, State goal) {
-    this(constraints, goal, new State(0, 0));
-  }
-
-  /**
-   * Calculate the correct position and velocity for the profile at a time t
-   * where the beginning of the profile was at time t = 0.
-   *
-   * @param t The time since the beginning of the profile.
-   */
-  @SuppressWarnings("ParameterName")
-  public State calculate(double t) {
-    State result = m_initial;
-
-    if (t < m_endAccel) {
-      result.velocity += t * m_constraints.maxAcceleration;
-      result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
-    } else if (t < m_endFullSpeed) {
-      result.velocity = m_constraints.maxVelocity;
-      result.position += (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration
-          / 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel);
-    } else if (t <= m_endDeccel) {
-      result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
-      double timeLeft = m_endDeccel - t;
-      result.position = m_goal.position - (m_goal.velocity + timeLeft
-          * m_constraints.maxAcceleration / 2.0) * timeLeft;
-    } else {
-      result = m_goal;
-    }
-
-    return direct(result);
-  }
-
-  /**
-   * Returns the time left until a target distance in the profile is reached.
-   *
-   * @param target The target distance.
-   */
-  public double timeLeftUntil(double target) {
-    double position = m_initial.position * m_direction;
-    double velocity = m_initial.velocity * m_direction;
-
-    double endAccel = m_endAccel * m_direction;
-    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
-
-    if (target < position) {
-      endAccel = -endAccel;
-      endFullSpeed = -endFullSpeed;
-      velocity = -velocity;
-    }
-
-    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;
-
-    double distToTarget = Math.abs(target - position);
-    if (distToTarget < 1e-6) {
-      return 0;
-    }
-
-    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
-
-    double deccelVelocity;
-    if (endAccel > 0) {
-      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
-    } else {
-      deccelVelocity = velocity;
-    }
-
-    double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
-
-    deccelDist = Math.max(deccelDist, 0);
-
-    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
-
-    if (accelDist > distToTarget) {
-      accelDist = distToTarget;
-      fullSpeedDist = 0;
-      deccelDist = 0;
-    } else if (accelDist + fullSpeedDist > distToTarget) {
-      fullSpeedDist = distToTarget - accelDist;
-      deccelDist = 0;
-    } else {
-      deccelDist = distToTarget - fullSpeedDist - accelDist;
-    }
-
-    double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
-        * accelDist))) / acceleration;
-
-    double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
-        + 2 * decceleration * deccelDist))) / decceleration;
-
-    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
-
-    return accelTime + fullSpeedTime + deccelTime;
-  }
-
-  /**
-   * Returns the total time the profile takes to reach the goal.
-   */
-  public double totalTime() {
-    return m_endDeccel;
-  }
-
-  /**
-   * Returns true if the profile has reached the goal.
-   *
-   * <p>The profile has reached the goal if the time since the profile started
-   * has exceeded the profile's total time.
-   *
-   * @param t The time since the beginning of the profile.
-   */
-  @SuppressWarnings("ParameterName")
-  public boolean isFinished(double t) {
-    return t >= totalTime();
-  }
-
-  /**
-   * Returns true if the profile inverted.
-   *
-   * <p>The profile is inverted if goal position is less than the initial position.
-   *
-   * @param initial     The initial state (usually the current state).
-   * @param goal        The desired state when the profile is complete.
-   */
-  private static boolean shouldFlipAcceleration(State initial, State goal) {
-    return initial.position > goal.position;
-  }
-
-  // Flip the sign of the velocity and position if the profile is inverted
-  private State direct(State in) {
-    State result = new State(in.position, in.velocity);
-    result.position = result.position * m_direction;
-    result.velocity = result.velocity * m_direction;
-    return result;
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
deleted file mode 100644
index f74e7b4..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-
-/**
- * A constraint on the maximum absolute centripetal acceleration allowed when
- * traversing a trajectory. The centripetal acceleration of a robot is defined
- * as the velocity squared divided by the radius of curvature.
- *
- * <p>Effectively, limiting the maximum centripetal acceleration will cause the
- * robot to slow down around tight turns, making it easier to track trajectories
- * with sharp turns.
- */
-public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
-  private final double m_maxCentripetalAccelerationMetersPerSecondSq;
-
-  /**
-   * Constructs a centripetal acceleration constraint.
-   *
-   * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
-   */
-  public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
-    m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
-  }
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // ac = v^2 / r
-    // k (curvature) = 1 / r
-
-    // therefore, ac = v^2 * k
-    // ac / k = v^2
-    // v = std::sqrt(ac / k)
-
-    return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq
-        / Math.abs(curvatureRadPerMeter));
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    // The acceleration of the robot has no impact on the centripetal acceleration
-    // of the robot.
-    return new MinMax();
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
deleted file mode 100644
index 9a57720..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
+++ /dev/null
@@ -1,76 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-
-/**
- * A class that enforces constraints on the differential drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for both sides of the drivetrain stay below a certain
- * limit.
- */
-public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
-  private final double m_maxSpeedMetersPerSecond;
-  private final DifferentialDriveKinematics m_kinematics;
-
-  /**
-   * Constructs a differential drive dynamics constraint.
-   *
-   * @param kinematics A kinematics component describing the drive geometry.
-   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
-   */
-  public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics,
-                                               double maxSpeedMetersPerSecond) {
-    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
-    m_kinematics = kinematics;
-  }
-
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // Create an object to represent the current chassis speeds.
-    var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond,
-        0, velocityMetersPerSecond * curvatureRadPerMeter);
-
-    // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
-
-    // Return the new linear chassis speed.
-    return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    return new MinMax();
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
deleted file mode 100644
index 71432bf..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
+++ /dev/null
@@ -1,105 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
-/**
- * A class that enforces constraints on differential drive voltage expenditure based on the motor
- * dynamics and the drive kinematics.  Ensures that the acceleration of any wheel of the robot
- * while following the trajectory is never higher than what can be achieved with the given
- * maximum voltage.
- */
-public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
-  private final SimpleMotorFeedforward m_feedforward;
-  private final DifferentialDriveKinematics m_kinematics;
-  private final double m_maxVoltage;
-
-  /**
-   * Creates a new DifferentialDriveVoltageConstraint.
-   *
-   * @param feedforward A feedforward component describing the behavior of the drive.
-   * @param kinematics  A kinematics component describing the drive geometry.
-   * @param maxVoltage  The maximum voltage available to the motors while following the path.
-   *                    Should be somewhat less than the nominal battery voltage (12V) to account
-   *                    for "voltage sag" due to current draw.
-   */
-  public DifferentialDriveVoltageConstraint(SimpleMotorFeedforward feedforward,
-                                            DifferentialDriveKinematics kinematics,
-                                            double maxVoltage) {
-    m_feedforward = requireNonNullParam(feedforward, "feedforward",
-                                        "DifferentialDriveVoltageConstraint");
-    m_kinematics = requireNonNullParam(kinematics, "kinematics",
-                                       "DifferentialDriveVoltageConstraint");
-    m_maxVoltage = maxVoltage;
-  }
-
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    return Double.POSITIVE_INFINITY;
-  }
-
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocityMetersPerSecond, 0,
-                                                                   velocityMetersPerSecond
-                                                                       * curvatureRadPerMeter));
-
-    double maxWheelSpeed = Math.max(wheelSpeeds.leftMetersPerSecond,
-                                    wheelSpeeds.rightMetersPerSecond);
-    double minWheelSpeed = Math.min(wheelSpeeds.leftMetersPerSecond,
-                                    wheelSpeeds.rightMetersPerSecond);
-
-    // Calculate maximum/minimum possible accelerations from motor dynamics
-    // and max/min wheel speeds
-    double maxWheelAcceleration =
-        m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
-    double minWheelAcceleration =
-        m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed);
-
-    // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
-    // increased by half of the trackwidth T.  Inner wheel has radius decreased
-    // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
-    // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2).
-    // Inner wheel is similar.
-
-    // sgn(speed) term added to correctly account for which wheel is on
-    // outside of turn:
-    // If moving forward, max acceleration constraint corresponds to wheel on outside of turn
-    // If moving backward, max acceleration constraint corresponds to wheel on inside of turn
-
-    double maxChassisAcceleration =
-        maxWheelAcceleration
-            / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
-            * Math.signum(velocityMetersPerSecond) / 2);
-    double minChassisAcceleration =
-        minWheelAcceleration
-            / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
-            * Math.signum(velocityMetersPerSecond) / 2);
-
-    // Negate acceleration of wheel on inside of turn if center of turn is inside of wheelbase
-    if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
-      if (velocityMetersPerSecond > 0) {
-        minChassisAcceleration = -minChassisAcceleration;
-      } else {
-        maxChassisAcceleration = -maxChassisAcceleration;
-      }
-    }
-
-    return new MinMax(minChassisAcceleration, maxChassisAcceleration);
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
deleted file mode 100644
index dfa2583..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-
-/**
- * A class that enforces constraints on the mecanum drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for all 4 wheels of the drivetrain stay below a certain
- * limit.
- */
-public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
-  private final double m_maxSpeedMetersPerSecond;
-  private final MecanumDriveKinematics m_kinematics;
-
-  /**
-   * Constructs a mecanum drive dynamics constraint.
-   *
-   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
-   */
-  public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics,
-                                               double maxSpeedMetersPerSecond) {
-    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
-    m_kinematics = kinematics;
-  }
-
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // Represents the velocity of the chassis in the x direction
-    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
-
-    // Represents the velocity of the chassis in the y direction
-    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
-
-    // Create an object to represent the current chassis speeds.
-    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
-        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
-
-    // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
-
-    // Convert normalized wheel speeds back to chassis speeds
-    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    // Return the new linear chassis speed.
-    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    return new MinMax();
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
deleted file mode 100644
index 8ca63f4..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-
-/**
- * A class that enforces constraints on the swerve drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for all 4 wheels of the drivetrain stay below a certain
- * limit.
- */
-public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
-  private final double m_maxSpeedMetersPerSecond;
-  private final SwerveDriveKinematics m_kinematics;
-
-  /**
-   * Constructs a swerve drive dynamics constraint.
-   *
-   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
-   */
-  public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics,
-                                               double maxSpeedMetersPerSecond) {
-    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
-    m_kinematics = kinematics;
-  }
-
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // Represents the velocity of the chassis in the x direction
-    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
-
-    // Represents the velocity of the chassis in the y direction
-    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
-
-    // Create an object to represent the current chassis speeds.
-    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
-        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
-
-    // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
-    SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
-
-    // Convert normalized wheel speeds back to chassis speeds
-    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    // Return the new linear chassis speed.
-    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    return new MinMax();
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
deleted file mode 100644
index b338c3f..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-
-/**
- * An interface for defining user-defined velocity and acceleration constraints
- * while generating trajectories.
- */
-public interface TrajectoryConstraint {
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                       double velocityMetersPerSecond);
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter,
-                                                double velocityMetersPerSecond);
-
-  /**
-   * Represents a minimum and maximum acceleration.
-   */
-  @SuppressWarnings("MemberName")
-  class MinMax {
-    public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
-    public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
-
-    /**
-     * Constructs a MinMax.
-     *
-     * @param minAccelerationMetersPerSecondSq The minimum acceleration.
-     * @param maxAccelerationMetersPerSecondSq The maximum acceleration.
-     */
-    public MinMax(double minAccelerationMetersPerSecondSq,
-                  double maxAccelerationMetersPerSecondSq) {
-      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
-      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
-    }
-
-    /**
-     * Constructs a MinMax with default values.
-     */
-    public MinMax() {
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
index e4d5b69..a576991 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,6 +18,98 @@
  */
 @SuppressWarnings("MemberName")
 public class Color {
+  private static final double kPrecision = Math.pow(2, -12);
+
+  public final double red;
+  public final double green;
+  public final double blue;
+
+  /**
+   * Constructs a Color.
+   *
+   * @param red   Red value (0-1)
+   * @param green Green value (0-1)
+   * @param blue  Blue value (0-1)
+   */
+  public Color(double red, double green, double blue) {
+    this.red = roundAndClamp(red);
+    this.green = roundAndClamp(green);
+    this.blue = roundAndClamp(blue);
+  }
+
+  /**
+   * Constructs a Color from a Color8Bit.
+   *
+   * @param color The color
+   */
+  public Color(Color8Bit color) {
+    this(color.red / 255.0,
+        color.green / 255.0,
+        color.blue / 255.0);
+  }
+
+  /**
+   * Creates a Color from HSV values.
+   *
+   * @param h The h value [0-180]
+   * @param s The s value [0-255]
+   * @param v The v value [0-255]
+   * @return The color
+   */
+  @SuppressWarnings("ParameterName")
+  public static Color fromHSV(int h, int s, int v) {
+    if (s == 0) {
+      return new Color(v / 255.0, v / 255.0, v / 255.0);
+    }
+
+    final int region = h / 30;
+    final int remainder = (h - (region * 30)) * 6;
+
+    final int p = (v * (255 - s)) >> 8;
+    final int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+    final int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+    switch (region) {
+      case 0:
+        return new Color(v / 255.0, t / 255.0, p / 255.0);
+      case 1:
+        return new Color(q / 255.0, v / 255.0, p / 255.0);
+      case 2:
+        return new Color(p / 255.0, v / 255.0, t / 255.0);
+      case 3:
+        return new Color(p / 255.0, q / 255.0, v / 255.0);
+      case 4:
+        return new Color(t / 255.0, p / 255.0, v / 255.0);
+      default:
+        return new Color(v / 255.0, p / 255.0, q / 255.0);
+    }
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (other == null || getClass() != other.getClass()) {
+      return false;
+    }
+
+    Color color = (Color) other;
+    return Double.compare(color.red, red) == 0
+        && Double.compare(color.green, green) == 0
+        && Double.compare(color.blue, blue) == 0;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(red, green, blue);
+  }
+
+  private static double roundAndClamp(double value) {
+    final var rounded = Math.round((value + kPrecision / 2) / kPrecision) * kPrecision;
+    return MathUtil.clamp(rounded, 0.0, 1.0);
+  }
+
   /*
    * FIRST Colors
    */
@@ -399,7 +491,7 @@
   /**
    * #20B2AA.
    */
-  public static final Color kLightSeagGeen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
+  public static final Color kLightSeaGreen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
 
   /**
    * #87CEFA.
@@ -414,7 +506,7 @@
   /**
    * #B0C4DE.
    */
-  public static final Color kLightSteellue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
+  public static final Color kLightSteelBlue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
 
   /**
    * #FFFFE0.
@@ -740,59 +832,4 @@
    * #9ACD32.
    */
   public static final Color kYellowGreen = new Color(0.6039216f, 0.8039216f, 0.19607843f);
-
-  public final double red;
-  public final double green;
-  public final double blue;
-
-  private static final double kPrecision = Math.pow(2, -12);
-
-  /**
-   * Constructs a Color.
-   *
-   * @param red   Red value (0-1)
-   * @param green Green value (0-1)
-   * @param blue  Blue value (0-1)
-   */
-  Color(double red, double green, double blue) {
-    this.red = roundAndClamp(red);
-    this.green = roundAndClamp(green);
-    this.blue = roundAndClamp(blue);
-  }
-
-  /**
-   * Constructs a Color from a Color8Bit.
-   *
-   * @param color The color
-   */
-  public Color(Color8Bit color) {
-    this(color.red / 255.0,
-        color.green / 255.0,
-        color.blue / 255.0);
-  }
-
-  @Override
-  public boolean equals(Object other) {
-    if (this == other) {
-      return true;
-    }
-    if (other == null || getClass() != other.getClass()) {
-      return false;
-    }
-
-    Color color = (Color) other;
-    return Double.compare(color.red, red) == 0
-        && Double.compare(color.green, green) == 0
-        && Double.compare(color.blue, blue) == 0;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(red, green, blue);
-  }
-
-  private static double roundAndClamp(double value) {
-    final var rounded = Math.round(value / kPrecision) * kPrecision;
-    return MathUtil.clamp(rounded, 0.0, 1.0);
-  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java
deleted file mode 100644
index 1df7d33..0000000
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Units.java
+++ /dev/null
@@ -1,104 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.util;
-
-/**
- * Utility class that converts between commonly used units in FRC.
- */
-public final class Units {
-  private static final double kInchesPerFoot = 12.0;
-  private static final double kMetersPerInch = 0.0254;
-  private static final double kSecondsPerMinute = 60;
-
-  /**
-   * Utility class, so constructor is private.
-   */
-  private Units() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
-
-  /**
-   * Converts given meters to feet.
-   *
-   * @param meters The meters to convert to feet.
-   * @return Feet converted from meters.
-   */
-  public static double metersToFeet(double meters) {
-    return metersToInches(meters) / kInchesPerFoot;
-  }
-
-  /**
-   * Converts given feet to meters.
-   *
-   * @param feet The feet to convert to meters.
-   * @return Meters converted from feet.
-   */
-  public static double feetToMeters(double feet) {
-    return inchesToMeters(feet * kInchesPerFoot);
-  }
-
-  /**
-   * Converts given meters to inches.
-   *
-   * @param meters The meters to convert to inches.
-   * @return Inches converted from meters.
-   */
-  public static double metersToInches(double meters) {
-    return meters / kMetersPerInch;
-  }
-
-  /**
-   * Converts given inches to meters.
-   *
-   * @param inches The inches to convert to meters.
-   * @return Meters converted from inches.
-   */
-  public static double inchesToMeters(double inches) {
-    return inches * kMetersPerInch;
-  }
-
-  /**
-   * Converts given degrees to radians.
-   *
-   * @param degrees The degrees to convert to radians.
-   * @return Radians converted from degrees.
-   */
-  public static double degreesToRadians(double degrees) {
-    return Math.toRadians(degrees);
-  }
-
-  /**
-   * Converts given radians to degrees.
-   *
-   * @param radians The radians to convert to degrees.
-   * @return Degrees converted from radians.
-   */
-  public static double radiansToDegrees(double radians) {
-    return Math.toDegrees(radians);
-  }
-
-  /**
-   * Converts rotations per minute to radians per second.
-   *
-   * @param rpm The rotations per minute to convert to radians per second.
-   * @return Radians per second converted from rotations per minute.
-   */
-  public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
-    return rpm * Math.PI / (kSecondsPerMinute / 2);
-  }
-
-  /**
-   * Converts radians per second to rotations per minute.
-   *
-   * @param radiansPerSecond The radians per second to convert to from rotations per minute.
-   * @return Rotations per minute converted from radians per second.
-   */
-  public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
-    return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
index 13f1651..0bd0113 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,12 +9,17 @@
 
 import java.util.stream.Stream;
 
+import org.junit.jupiter.api.Test;
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.Arguments;
 import org.junit.jupiter.params.provider.MethodSource;
 
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.params.provider.Arguments.arguments;
 
 class AddressableLEDBufferTest {
@@ -51,4 +56,26 @@
         arguments(120, 255, 128, 0, 0, 128) // Navy
     );
   }
+
+  @Test
+  void getColorTest() {
+    AddressableLEDBuffer buffer = new AddressableLEDBuffer(4);
+    final Color8Bit denimColor8Bit = new Color8Bit(Color.kDenim);
+    final Color8Bit firstBlueColor8Bit = new Color8Bit(Color.kFirstBlue);
+    final Color8Bit firstRedColor8Bit = new Color8Bit(Color.kFirstRed);
+
+    buffer.setLED(0, Color.kFirstBlue);
+    buffer.setLED(1, denimColor8Bit);
+    buffer.setLED(2, Color.kFirstRed);
+    buffer.setLED(3, Color.kFirstBlue);
+
+    assertTrue(buffer.getLED(0).equals(Color.kFirstBlue));
+    assertTrue(buffer.getLED(1).equals(Color.kDenim));
+    assertTrue(buffer.getLED(2).equals(Color.kFirstRed));
+    assertTrue(buffer.getLED(3).equals(Color.kFirstBlue));
+    assertTrue(buffer.getLED8Bit(0).equals(firstBlueColor8Bit));
+    assertTrue(buffer.getLED8Bit(1).equals(denimColor8Bit));
+    assertTrue(buffer.getLED8Bit(2).equals(firstRedColor8Bit));
+    assertTrue(buffer.getLED8Bit(3).equals(firstBlueColor8Bit));
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java
new file mode 100644
index 0000000..50e16c8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.stream.Stream;
+
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class DriverStationTest {
+  @ParameterizedTest
+  @MethodSource("isConnectedProvider")
+  void testIsConnected(int axisCount, int buttonCount, int povCount, boolean expected) {
+    DriverStationSim.setJoystickAxisCount(1, axisCount);
+    DriverStationSim.setJoystickButtonCount(1, buttonCount);
+    DriverStationSim.setJoystickPOVCount(1, povCount);
+
+    DriverStationSim.notifyNewData();
+
+    assertEquals(expected, DriverStation.getInstance().isJoystickConnected(1));
+  }
+
+  static Stream<Arguments> isConnectedProvider() {
+    return Stream.of(
+      arguments(0, 0, 0, false),
+      arguments(1, 0, 0, true),
+      arguments(0, 1, 0, true),
+      arguments(0, 0, 1, true),
+      arguments(1, 1, 1, true),
+      arguments(4, 10, 1, true)
+    );
+  }
+
+  @MethodSource("connectionWarningProvider")
+  void testConnectionWarnings(boolean fms, boolean silence, boolean expected) {
+    DriverStationSim.setFmsAttached(fms);
+    DriverStationSim.notifyNewData();
+
+    DriverStation.getInstance().silenceJoystickConnectionWarning(silence);
+    assertEquals(expected,
+        DriverStation.getInstance().isJoystickConnectionWarningSilenced());
+  }
+
+  static Stream<Arguments> connectionWarningProvider() {
+    return Stream.of(
+      arguments(false, true, true),
+      arguments(false, false, false),
+      arguments(true, true, false),
+      arguments(true, false, false)
+    );
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java
new file mode 100644
index 0000000..7b1cfbb
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.JoystickSim;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class JoystickTest {
+  @Test
+  void testGetX() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setX(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getX(), 0.001);
+
+    joysim.setX(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getX(), 0.001);
+  }
+
+  @Test
+  void testGetY() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setY(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getY(), 0.001);
+
+    joysim.setY(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getY(), 0.001);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
deleted file mode 100644
index 3953c4c..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
+++ /dev/null
@@ -1,116 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.Random;
-import java.util.function.DoubleFunction;
-import java.util.stream.Stream;
-
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.params.ParameterizedTest;
-import org.junit.jupiter.params.provider.Arguments;
-import org.junit.jupiter.params.provider.MethodSource;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-import static org.junit.jupiter.params.provider.Arguments.arguments;
-
-class LinearFilterTest {
-  private static final double kFilterStep = 0.005;
-  private static final double kFilterTime = 2.0;
-  private static final double kSinglePoleIIRTimeConstant = 0.015915;
-  private static final double kHighPassTimeConstant = 0.006631;
-  private static final int kMovAvgTaps = 6;
-
-  private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
-  private static final double kHighPassExpectedOutput = 10.074717;
-  private static final double kMovAvgExpectedOutput = -10.191644;
-
-  @SuppressWarnings("ParameterName")
-  private static double getData(double t) {
-    return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
-  }
-
-  @SuppressWarnings("ParameterName")
-  private static double getPulseData(double t) {
-    if (Math.abs(t - 1.0) < 0.001) {
-      return 1.0;
-    } else {
-      return 0.0;
-    }
-  }
-
-  @Test
-  void illegalTapNumberTest() {
-    assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
-  }
-
-  /**
-   * Test if the filter reduces the noise produced by a signal generator.
-   */
-  @ParameterizedTest
-  @MethodSource("noiseFilterProvider")
-  void noiseReduceTest(final LinearFilter filter) {
-    double noiseGenError = 0.0;
-    double filterError = 0.0;
-
-    final Random gen = new Random();
-    final double kStdDev = 10.0;
-
-    for (double t = 0; t < kFilterTime; t += kFilterStep) {
-      final double theory = getData(t);
-      final double noise = gen.nextGaussian() * kStdDev;
-      filterError += Math.abs(filter.calculate(theory + noise) - theory);
-      noiseGenError += Math.abs(noise - theory);
-    }
-
-    assertTrue(noiseGenError > filterError,
-        "Filter should have reduced noise accumulation from " + noiseGenError
-            + " but failed. The filter error was " + filterError);
-  }
-
-  static Stream<LinearFilter> noiseFilterProvider() {
-    return Stream.of(
-        LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
-        LinearFilter.movingAverage(kMovAvgTaps)
-    );
-  }
-
-  /**
-   * Test if the linear filters produce consistent output for a given data set.
-   */
-  @ParameterizedTest
-  @MethodSource("outputFilterProvider")
-  void outputTest(final LinearFilter filter, final DoubleFunction<Double> data,
-                  final double expectedOutput) {
-    double filterOutput = 0.0;
-    for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
-      filterOutput = filter.calculate(data.apply(t));
-    }
-
-    assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
-  }
-
-  static Stream<Arguments> outputFilterProvider() {
-    return Stream.of(
-        arguments(LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
-            (DoubleFunction<Double>) LinearFilterTest::getData,
-            kSinglePoleIIRExpectedOutput),
-        arguments(LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
-            (DoubleFunction<Double>) LinearFilterTest::getData,
-            kHighPassExpectedOutput),
-        arguments(LinearFilter.movingAverage(kMovAvgTaps),
-            (DoubleFunction<Double>) LinearFilterTest::getData,
-            kMovAvgExpectedOutput),
-        arguments(LinearFilter.movingAverage(kMovAvgTaps),
-            (DoubleFunction<Double>) LinearFilterTest::getPulseData,
-            0.0)
-    );
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
deleted file mode 100644
index 00b100d..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
+++ /dev/null
@@ -1,64 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-public class MedianFilterTest {
-  @Test
-  void medianFilterNotFullTestEven() {
-    MedianFilter filter = new MedianFilter(10);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(4);
-
-    assertEquals(3.5, filter.calculate(1000));
-  }
-
-  @Test
-  void medianFilterNotFullTestOdd() {
-    MedianFilter filter = new MedianFilter(10);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(4);
-    filter.calculate(7);
-
-    assertEquals(4, filter.calculate(1000));
-  }
-
-  @Test
-  void medianFilterFullTestEven() {
-    MedianFilter filter = new MedianFilter(6);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(0);
-    filter.calculate(5);
-    filter.calculate(4);
-    filter.calculate(1000);
-
-    assertEquals(4.5, filter.calculate(99));
-  }
-
-  @Test
-  void medianFilterFullTestOdd() {
-    MedianFilter filter = new MedianFilter(5);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(5);
-    filter.calculate(4);
-    filter.calculate(1000);
-
-    assertEquals(5, filter.calculate(99));
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
index d97b352..7406ca9 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
 
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.sim.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 
 public final class MockHardwareExtension implements BeforeAllCallback {
   private static ExtensionContext getRoot(ExtensionContext context) {
@@ -21,7 +21,7 @@
 
   @Override
   public void beforeAll(ExtensionContext context) {
-    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initalized", key -> {
+    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initialized", key -> {
       initializeHardware();
       return true;
     }, Boolean.class);
@@ -29,12 +29,9 @@
 
   private void initializeHardware() {
     HAL.initialize(500, 0);
-    DriverStationSim dsSim = new DriverStationSim();
-    dsSim.setDsAttached(true);
-    dsSim.setAutonomous(false);
-    dsSim.setEnabled(true);
-    dsSim.setTest(true);
-
-
+    DriverStationSim.setDsAttached(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setTest(true);
   }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
new file mode 100644
index 0000000..6942ca0
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
@@ -0,0 +1,503 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.atomic.AtomicInteger;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class TimedRobotTest {
+  class MockRobot extends TimedRobot {
+    public final AtomicInteger m_robotInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_simulationInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_disabledInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_testInitCount = new AtomicInteger(0);
+
+    public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_simulationPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_disabledPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_testPeriodicCount = new AtomicInteger(0);
+
+    @Override
+    public void robotInit() {
+      m_robotInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void simulationInit() {
+      m_simulationInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void disabledInit() {
+      m_disabledInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousInit() {
+      m_autonomousInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopInit() {
+      m_teleopInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void testInit() {
+      m_testInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void robotPeriodic() {
+      m_robotPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+      m_simulationPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void disabledPeriodic() {
+      m_disabledPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousPeriodic() {
+      m_autonomousPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopPeriodic() {
+      m_teleopPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void testPeriodic() {
+      m_testPeriodicCount.addAndGet(1);
+    }
+  }
+
+  @BeforeEach
+  void setup() {
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void disabledTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(2, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void autonomousTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(true);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(2, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void teleopTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(1, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(2, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void testTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(true);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(1, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(2, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void addPeriodicTest() {
+    MockRobot robot = new MockRobot();
+
+    final AtomicInteger callbackCount = new AtomicInteger(0);
+    robot.addPeriodic(() -> {
+      callbackCount.addAndGet(1);
+    }, 0.01);
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, callbackCount.get());
+
+    SimHooks.stepTiming(0.01);
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, callbackCount.get());
+
+    SimHooks.stepTiming(0.01);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(2, callbackCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void addPeriodicWithOffsetTest() {
+    MockRobot robot = new MockRobot();
+
+    final AtomicInteger callbackCount = new AtomicInteger(0);
+    robot.addPeriodic(() -> {
+      callbackCount.addAndGet(1);
+    }, 0.01, 0.005);
+
+    // Expirations in this test (ms)
+    //
+    // Robot | Callback
+    // ================
+    //    20 |      15
+    //    40 |      25
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, callbackCount.get());
+
+    SimHooks.stepTiming(0.0075);
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, callbackCount.get());
+
+    SimHooks.stepTiming(0.0075);
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, callbackCount.get());
+
+    SimHooks.stepTiming(0.005);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, callbackCount.get());
+
+    SimHooks.stepTiming(0.005);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(2, callbackCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
index b956446..d99d7db 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,220 +9,184 @@
 
 import java.util.concurrent.atomic.AtomicInteger;
 
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.condition.DisabledOnOs;
-import org.junit.jupiter.api.condition.OS;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-@DisabledOnOs(OS.MAC)
 class WatchdogTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
   @Test
+  @ResourceLock("timing")
   void enableDisableTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    final Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1));
+    try (Watchdog watchdog = new Watchdog(0.4, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
+      // Run 1
+      watchdog.enable();
+      SimHooks.stepTiming(0.2);
+      watchdog.disable();
 
-    System.out.println("Run 1");
-    watchdog.enable();
-    try {
-      Thread.sleep(200);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+      // Run 2
+      watchdogCounter.set(0);
+      watchdog.enable();
+      SimHooks.stepTiming(0.4);
+      watchdog.disable();
+
+      assertEquals(1, watchdogCounter.get(),
+          "Watchdog either didn't trigger or triggered more than once");
+
+      // Run 3
+      watchdogCounter.set(0);
+      watchdog.enable();
+      SimHooks.stepTiming(1.0);
+      watchdog.disable();
+
+      assertEquals(1, watchdogCounter.get(),
+          "Watchdog either didn't trigger or triggered more than once");
     }
-    watchdog.disable();
-
-    assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
-
-    System.out.println("Run 2");
-    watchdogCounter.set(0);
-    watchdog.enable();
-    try {
-      Thread.sleep(600);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog.disable();
-
-    assertEquals(1, watchdogCounter.get(),
-        "Watchdog either didn't trigger or triggered more than once");
-
-    // Run 3
-    watchdogCounter.set(0);
-    watchdog.enable();
-    try {
-      Thread.sleep(1000);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog.disable();
-
-    assertEquals(1, watchdogCounter.get(),
-        "Watchdog either didn't trigger or triggered more than once");
-
-    watchdog.close();
   }
 
   @Test
+  @ResourceLock("timing")
   void resetTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    final Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1));
+    try (Watchdog watchdog = new Watchdog(0.4, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
+      watchdog.enable();
+      SimHooks.stepTiming(0.2);
+      watchdog.reset();
+      SimHooks.stepTiming(0.2);
+      watchdog.disable();
 
-    watchdog.enable();
-    try {
-      Thread.sleep(200);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
     }
-    watchdog.reset();
-    try {
-      Thread.sleep(200);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog.disable();
-
-    assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
-
-    watchdog.close();
   }
 
   @Test
+  @ResourceLock("timing")
   void setTimeoutTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    final Watchdog watchdog = new Watchdog(1.0, () -> watchdogCounter.addAndGet(1));
+    try (Watchdog watchdog = new Watchdog(1.0, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
+      watchdog.enable();
+      SimHooks.stepTiming(0.2);
+      watchdog.setTimeout(0.2);
 
-    watchdog.enable();
-    try {
-      Thread.sleep(200);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
+      assertEquals(0.2, watchdog.getTimeout());
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+      SimHooks.stepTiming(0.3);
+      watchdog.disable();
+
+      assertEquals(1, watchdogCounter.get(),
+          "Watchdog either didn't trigger or triggered more than once");
     }
-    watchdog.setTimeout(0.2);
-
-    assertEquals(0.2, watchdog.getTimeout());
-    assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
-
-    try {
-      Thread.sleep(300);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog.disable();
-
-    assertEquals(1, watchdogCounter.get(),
-        "Watchdog either didn't trigger or triggered more than once");
-
-    watchdog.close();
   }
 
   @Test
+  @ResourceLock("timing")
   void isExpiredTest() {
-    final Watchdog watchdog = new Watchdog(0.2, () -> {
-    });
-    assertFalse(watchdog.isExpired());
-    watchdog.enable();
+    try (Watchdog watchdog = new Watchdog(0.2, () -> {
+    })) {
+      assertFalse(watchdog.isExpired());
+      watchdog.enable();
 
-    assertFalse(watchdog.isExpired());
-    try {
-      Thread.sleep(300);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
+      assertFalse(watchdog.isExpired());
+      SimHooks.stepTiming(0.3);
+      assertTrue(watchdog.isExpired());
+
+      watchdog.disable();
+      assertTrue(watchdog.isExpired());
+
+      watchdog.reset();
+      assertFalse(watchdog.isExpired());
     }
-    assertTrue(watchdog.isExpired());
-
-    watchdog.disable();
-    assertTrue(watchdog.isExpired());
-
-    watchdog.reset();
-    assertFalse(watchdog.isExpired());
-
-    watchdog.close();
   }
 
   @Test
+  @ResourceLock("timing")
   void epochsTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    final Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1));
+    try (Watchdog watchdog = new Watchdog(0.4, () -> {
+      watchdogCounter.addAndGet(1);
+    })) {
+      // Run 1
+      watchdog.enable();
+      watchdog.addEpoch("Epoch 1");
+      SimHooks.stepTiming(0.1);
+      watchdog.addEpoch("Epoch 2");
+      SimHooks.stepTiming(0.1);
+      watchdog.addEpoch("Epoch 3");
+      watchdog.disable();
 
-    System.out.println("Run 1");
-    watchdog.enable();
-    watchdog.addEpoch("Epoch 1");
-    try {
-      Thread.sleep(100);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
+
+      // Run 2
+      watchdog.enable();
+      watchdog.addEpoch("Epoch 1");
+      SimHooks.stepTiming(0.2);
+      watchdog.reset();
+      SimHooks.stepTiming(0.2);
+      watchdog.addEpoch("Epoch 2");
+      watchdog.disable();
+
+      assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
     }
-    watchdog.addEpoch("Epoch 2");
-    try {
-      Thread.sleep(100);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog.addEpoch("Epoch 3");
-    watchdog.disable();
-
-    assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
-
-    System.out.println("Run 2");
-    watchdog.enable();
-    watchdog.addEpoch("Epoch 1");
-    try {
-      Thread.sleep(200);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog.reset();
-    try {
-      Thread.sleep(200);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog.addEpoch("Epoch 2");
-    watchdog.disable();
-
-    assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
-
-    watchdog.close();
   }
 
   @Test
+  @ResourceLock("timing")
   void multiWatchdogTest() {
     final AtomicInteger watchdogCounter1 = new AtomicInteger(0);
     final AtomicInteger watchdogCounter2 = new AtomicInteger(0);
 
-    final Watchdog watchdog1 = new Watchdog(0.2, () -> watchdogCounter1.addAndGet(1));
-    final Watchdog watchdog2 = new Watchdog(0.6, () -> watchdogCounter2.addAndGet(1));
+    try (Watchdog watchdog1 = new Watchdog(0.2, () -> {
+      watchdogCounter1.addAndGet(1);
+    });
+        Watchdog watchdog2 = new Watchdog(0.6, () -> {
+          watchdogCounter2.addAndGet(1);
+        })) {
+      watchdog2.enable();
+      SimHooks.stepTiming(0.25);
+      assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
+      assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
 
-    watchdog2.enable();
-    try {
-      Thread.sleep(200);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
+      // Sleep enough such that only the watchdog enabled later times out first
+      watchdog1.enable();
+      SimHooks.stepTiming(0.25);
+      watchdog1.disable();
+      watchdog2.disable();
+
+      assertEquals(1, watchdogCounter1.get(),
+          "Watchdog either didn't trigger or triggered more than once");
+      assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
     }
-    assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
-    assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
-
-    // Sleep enough such that only the watchdog enabled later times out first
-    watchdog1.enable();
-    try {
-      Thread.sleep(300);
-    } catch (InterruptedException ex) {
-      Thread.currentThread().interrupt();
-    }
-    watchdog1.disable();
-    watchdog2.disable();
-
-    assertEquals(1, watchdogCounter1.get(),
-        "Watchdog either didn't trigger or triggered more than once");
-    assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
-
-    watchdog1.close();
-    watchdog2.close();
   }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java
new file mode 100644
index 0000000..34fce9a
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.XboxControllerSim;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class XboxControllerTest {
+  @Test
+  void testGetX() {
+    HAL.initialize(500, 0);
+    XboxController joy = new XboxController(2);
+    XboxControllerSim joysim = new XboxControllerSim(joy);
+
+    joysim.setX(XboxController.Hand.kLeft, 0.35);
+    joysim.setX(XboxController.Hand.kRight, 0.45);
+    joysim.notifyNewData();
+    assertEquals(0.35, joy.getX(XboxController.Hand.kLeft), 0.001);
+    assertEquals(0.45, joy.getX(XboxController.Hand.kRight), 0.001);
+  }
+
+  @Test
+  void testGetBumper() {
+    HAL.initialize(500, 0);
+    XboxController joy = new XboxController(2);
+    XboxControllerSim joysim = new XboxControllerSim(joy);
+
+    joysim.setBumper(XboxController.Hand.kLeft, false);
+    joysim.setBumper(XboxController.Hand.kRight, true);
+    joysim.notifyNewData();
+    assertFalse(joy.getBumper(XboxController.Hand.kLeft));
+    assertTrue(joy.getBumper(XboxController.Hand.kRight));
+    // need to call pressed and released to clear flags
+    joy.getBumperPressed(XboxController.Hand.kLeft);
+    joy.getBumperReleased(XboxController.Hand.kLeft);
+    joy.getBumperPressed(XboxController.Hand.kRight);
+    joy.getBumperReleased(XboxController.Hand.kRight);
+
+    joysim.setBumper(XboxController.Hand.kLeft, true);
+    joysim.setBumper(XboxController.Hand.kRight, false);
+    joysim.notifyNewData();
+    assertTrue(joy.getBumper(XboxController.Hand.kLeft));
+    assertTrue(joy.getBumperPressed(XboxController.Hand.kLeft));
+    assertFalse(joy.getBumperReleased(XboxController.Hand.kLeft));
+    assertFalse(joy.getBumper(XboxController.Hand.kRight));
+    assertFalse(joy.getBumperPressed(XboxController.Hand.kRight));
+    assertTrue(joy.getBumperReleased(XboxController.Hand.kRight));
+  }
+
+  @Test
+  void testGetAButton() {
+    HAL.initialize(500, 0);
+    XboxController joy = new XboxController(2);
+    XboxControllerSim joysim = new XboxControllerSim(joy);
+
+    joysim.setAButton(false);
+    joysim.notifyNewData();
+    assertFalse(joy.getAButton());
+    // need to call pressed and released to clear flags
+    joy.getAButtonPressed();
+    joy.getAButtonReleased();
+
+    joysim.setAButton(true);
+    joysim.notifyNewData();
+    assertTrue(joy.getAButton());
+    assertTrue(joy.getAButtonPressed());
+    assertFalse(joy.getAButtonReleased());
+
+    joysim.setAButton(false);
+    joysim.notifyNewData();
+    assertFalse(joy.getAButton());
+    assertFalse(joy.getAButtonPressed());
+    assertTrue(joy.getAButtonReleased());
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java
new file mode 100644
index 0000000..307eccd
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ControllerUtilTest {
+  @Test
+  void testGetModulusError() {
+    // Test symmetric range
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -180.0, 180.0));
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
+    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
+
+    // Test range start at zero
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0, 0.0, 360.0));
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0 + 360, 0.0, 360.0));
+
+    // Test asymmetric range that doesn't start at zero
+    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -170.0, 190.0));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
new file mode 100644
index 0000000..2bf7900
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class HolonomicDriveControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  void testReachesReference() {
+    HolonomicDriveController controller = new HolonomicDriveController(
+        new PIDController(1.0, 0.0, 0.0),
+        new PIDController(1.0, 0.0, 0.0),
+        new ProfiledPIDController(1.0, 0.0, 0.0,
+            new TrapezoidProfile.Constraints(6.28, 3.14))
+    );
+    Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    List<Pose2d> waypoints = new ArrayList<>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
+
+    TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
+    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final double kDt = 0.02;
+    final double kTotalTime = trajectory.getTotalTimeSeconds();
+
+    for (int i = 0; i < (kTotalTime / kDt); i++) {
+      Trajectory.State state = trajectory.sample(kDt * i);
+      ChassisSpeeds output = controller.calculate(robotPose, state, new Rotation2d());
+
+      robotPose = robotPose.exp(new Twist2d(output.vxMetersPerSecond * kDt,
+          output.vyMetersPerSecond * kDt, output.omegaRadiansPerSecond * kDt));
+    }
+
+    final List<Trajectory.State> states = trajectory.getStates();
+    final Pose2d endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final Pose2d finalRobotPose = robotPose;
+
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(),
+            kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(),
+            kTolerance),
+        () -> assertEquals(0.0,
+            MathUtil.normalizeAngle(finalRobotPose.getRotation().getRadians()),
+            kAngularTolerance)
+    );
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
index cd2aba4..b2d43c6 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,36 +19,38 @@
 
   @Test
   void initialToleranceTest() {
-    var controller = new PIDController(0.05, 0.0, 0.0);
-    controller.enableContinuousInput(-kRange / 2, kRange / 2);
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
-    assertTrue(controller.atSetpoint());
+      assertTrue(controller.atSetpoint());
+    }
   }
 
   @Test
   void absoluteToleranceTest() {
-    var controller = new PIDController(0.05, 0.0, 0.0);
-    controller.enableContinuousInput(-kRange / 2, kRange / 2);
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
 
-    controller.setTolerance(kTolerance);
-    controller.setSetpoint(kSetpoint);
+      controller.setTolerance(kTolerance);
+      controller.setSetpoint(kSetpoint);
 
-    controller.calculate(0.0);
+      controller.calculate(0.0);
 
-    assertFalse(controller.atSetpoint(),
-        "Error was in tolerance when it should not have been. Error was " + controller
-            .getPositionError());
+      assertFalse(controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+          + controller.getPositionError());
 
-    controller.calculate(kSetpoint + kTolerance / 2);
+      controller.calculate(kSetpoint + kTolerance / 2);
 
-    assertTrue(controller.atSetpoint(),
-        "Error was not in tolerance when it should have been. Error was " + controller
-            .getPositionError());
+      assertTrue(controller.atSetpoint(),
+          "Error was not in tolerance when it should have been. Error was "
+          + controller.getPositionError());
 
-    controller.calculate(kSetpoint + 10 * kTolerance);
+      controller.calculate(kSetpoint + 10 * kTolerance);
 
-    assertFalse(controller.atSetpoint(),
-        "Error was in tolerance when it should not have been. Error was " + controller
-            .getPositionError());
+      assertFalse(controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+          + controller.getPositionError());
+    }
   }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java
new file mode 100644
index 0000000..9fca2a4
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class ProfiledPIDInputOutputTest {
+  private ProfiledPIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new ProfiledPIDController(0, 0, 0,
+        new TrapezoidProfile.Constraints(360, 180));
+  }
+
+  @Test
+  void continuousInputTest1() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+
+    final double kSetpoint = -179.0;
+    final double kMeasurement = -179.0;
+    final double kGoal = 179.0;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < 180.0);
+  }
+
+  @Test
+  void continuousInputTest2() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.4826633343199735;
+    final double kMeasurement = -3.1352207333939606;
+    final double kGoal = -3.534162788601621;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest3() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.5176604690006377;
+    final double kMeasurement = 3.1191729343822456;
+    final double kGoal = 2.709680418117445;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
index 2bef50d..a2b6e22 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj.geometry.Twist2d;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.MathUtil;
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
@@ -24,16 +25,6 @@
   private static final double kTolerance = 1 / 12.0;
   private static final double kAngularTolerance = Math.toRadians(2);
 
-  private static double boundRadians(double value) {
-    while (value > Math.PI) {
-      value -= Math.PI * 2;
-    }
-    while (value <= -Math.PI) {
-      value += Math.PI * 2;
-    }
-    return value;
-  }
-
   @Test
   @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
@@ -63,12 +54,12 @@
     // must be final or effectively final.
     final var finalRobotPose = robotPose;
     assertAll(
-        () -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(),
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(),
             kTolerance),
-        () -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(),
             kTolerance),
         () -> assertEquals(0.0,
-            boundRadians(endPose.getRotation().getRadians()
+            MathUtil.normalizeAngle(endPose.getRotation().getRadians()
                 - finalRobotPose.getRotation().getRadians()),
             kAngularTolerance)
     );

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
deleted file mode 100644
index 066716e..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
+++ /dev/null
@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Pose2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testTransformBy() {
-    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
-    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
-        Rotation2d.fromDegrees(5.0));
-
-    var transformed = initial.plus(transformation);
-
-    assertAll(
-        () -> assertEquals(transformed.getTranslation().getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transformed.getTranslation().getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRelativeTo() {
-    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
-    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
-
-    var finalRelativeToInitial = last.relativeTo(initial);
-
-    assertAll(
-        () -> assertEquals(finalRelativeToInitial.getTranslation().getX(), 5.0 * Math.sqrt(2.0),
-            kEpsilon),
-        () -> assertEquals(finalRelativeToInitial.getTranslation().getY(), 0.0, kEpsilon),
-        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testEquality() {
-    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
-    var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
-    var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
-    assertNotEquals(one, two);
-  }
-
-  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));
-
-    final var transform = last.minus(initial);
-
-    assertAll(
-        () -> assertEquals(transform.getTranslation().getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transform.getTranslation().getY(), 0.0, kEpsilon),
-        () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)
-    );
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
deleted file mode 100644
index 6c4b1b3..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
+++ /dev/null
@@ -1,81 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Rotation2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testRadiansToDegrees() {
-    var one = new Rotation2d(Math.PI / 3);
-    var two = new Rotation2d(Math.PI / 4);
-
-    assertAll(
-        () -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
-        () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRadiansAndDegrees() {
-    var one = Rotation2d.fromDegrees(45.0);
-    var two = Rotation2d.fromDegrees(30.0);
-
-    assertAll(
-        () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
-        () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRotateByFromZero() {
-    var zero = new Rotation2d();
-    var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
-
-    assertAll(
-        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
-        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRotateByNonZero() {
-    var rot = Rotation2d.fromDegrees(90.0);
-    rot = rot.plus(Rotation2d.fromDegrees(30.0));
-
-    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
-  }
-
-  @Test
-  void testMinus() {
-    var one = Rotation2d.fromDegrees(70.0);
-    var two = Rotation2d.fromDegrees(30.0);
-
-    assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
-  }
-
-  @Test
-  void testEquality() {
-    var one = Rotation2d.fromDegrees(43.0);
-    var two = Rotation2d.fromDegrees(43.0);
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = Rotation2d.fromDegrees(43.0);
-    var two = Rotation2d.fromDegrees(43.5);
-    assertNotEquals(one, two);
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
deleted file mode 100644
index b4e2947..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
+++ /dev/null
@@ -1,115 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Translation2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testSum() {
-    var one = new Translation2d(1.0, 3.0);
-    var two = new Translation2d(2.0, 5.0);
-
-    var sum = one.plus(two);
-
-    assertAll(
-        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
-        () -> assertEquals(sum.getY(), 8.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testDifference() {
-    var one = new Translation2d(1.0, 3.0);
-    var two = new Translation2d(2.0, 5.0);
-
-    var difference = one.minus(two);
-
-    assertAll(
-        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
-        () -> assertEquals(difference.getY(), -2.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRotateBy() {
-    var another = new Translation2d(3.0, 0.0);
-    var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
-
-    assertAll(
-        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
-        () -> assertEquals(rotated.getY(), 3.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testMultiplication() {
-    var original = new Translation2d(3.0, 5.0);
-    var mult = original.times(3);
-
-    assertAll(
-        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
-        () -> assertEquals(mult.getY(), 15.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testDivision() {
-    var original = new Translation2d(3.0, 5.0);
-    var div = original.div(2);
-
-    assertAll(
-        () -> assertEquals(div.getX(), 1.5, kEpsilon),
-        () -> assertEquals(div.getY(), 2.5, kEpsilon)
-    );
-  }
-
-  @Test
-  void testNorm() {
-    var one = new Translation2d(3.0, 5.0);
-    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
-  }
-
-  @Test
-  void testDistance() {
-    var one = new Translation2d(1, 1);
-    var two = new Translation2d(6, 6);
-    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
-  }
-
-  @Test
-  void testUnaryMinus() {
-    var original = new Translation2d(-4.5, 7);
-    var inverted = original.unaryMinus();
-
-    assertAll(
-        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
-        () -> assertEquals(inverted.getY(), -7, kEpsilon)
-    );
-  }
-
-  @Test
-  void testEquality() {
-    var one = new Translation2d(9, 5.5);
-    var two = new Translation2d(9, 5.5);
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = new Translation2d(9, 5.5);
-    var two = new Translation2d(9, 5.7);
-    assertNotEquals(one, two);
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
deleted file mode 100644
index 903c436..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
+++ /dev/null
@@ -1,81 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Twist2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testStraightLineTwist() {
-    var straight = new Twist2d(5.0, 0.0, 0.0);
-    var straightPose = new Pose2d().exp(straight);
-
-    assertAll(
-        () -> assertEquals(straightPose.getTranslation().getX(), 5.0, kEpsilon),
-        () -> assertEquals(straightPose.getTranslation().getY(), 0.0, kEpsilon),
-        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testQuarterCirleTwist() {
-    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
-    var quarterCirclePose = new Pose2d().exp(quarterCircle);
-
-    assertAll(
-        () -> assertEquals(quarterCirclePose.getTranslation().getX(), 5.0, kEpsilon),
-        () -> assertEquals(quarterCirclePose.getTranslation().getY(), 5.0, kEpsilon),
-        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testDiagonalNoDtheta() {
-    var diagonal = new Twist2d(2.0, 2.0, 0.0);
-    var diagonalPose = new Pose2d().exp(diagonal);
-
-    assertAll(
-        () -> assertEquals(diagonalPose.getTranslation().getX(), 2.0, kEpsilon),
-        () -> assertEquals(diagonalPose.getTranslation().getY(), 2.0, kEpsilon),
-        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testEquality() {
-    var one = new Twist2d(5, 1, 3);
-    var two = new Twist2d(5, 1, 3);
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = new Twist2d(5, 1, 3);
-    var two = new Twist2d(5, 1.2, 3);
-    assertNotEquals(one, two);
-  }
-
-  void testPose2dLog() {
-    final var start = new Pose2d();
-    final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
-
-    final var twist = start.log(end);
-
-    assertAll(
-        () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
-        () -> assertEquals(twist.dy, 0.0, kEpsilon),
-        () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)
-    );
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
index 98b8489..3887d91 100644
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.MatchInfoData;
-import edu.wpi.first.hal.sim.mockdata.DriverStationDataJNI;
+import edu.wpi.first.hal.simulation.DriverStationDataJNI;
 import edu.wpi.first.wpilibj.DriverStation.MatchType;
 
 import static org.junit.jupiter.api.Assertions.assertAll;

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
deleted file mode 100644
index 729d7b8..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class ChassisSpeedsTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testFieldRelativeConstruction() {
-    final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
-        1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)
-    );
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
deleted file mode 100644
index e484eab..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
+++ /dev/null
@@ -1,88 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class DifferentialDriveKinematicsTest {
-  private static final double kEpsilon = 1E-9;
-  private final DifferentialDriveKinematics m_kinematics
-      = new DifferentialDriveKinematics(0.381 * 2);
-
-  @Test
-  void testInverseKinematicsForZeros() {
-    var chassisSpeeds = new ChassisSpeeds();
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-
-    assertAll(
-        () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testForwardKinematicsForZeros() {
-    var wheelSpeeds = new DifferentialDriveWheelSpeeds();
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testInverseKinematicsForStraightLine() {
-    var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-
-    assertAll(
-        () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
-        () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testForwardKinematicsForStraightLine() {
-    var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testInverseKinematicsForRotateInPlace() {
-    var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-
-    assertAll(
-        () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
-        () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testForwardKinematicsForRotateInPlace() {
-    var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
deleted file mode 100644
index aabc455..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class DifferentialDriveOdometryTest {
-  private static final double kEpsilon = 1E-9;
-  private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
-      new Rotation2d());
-
-  @Test
-  void testOdometryWithEncoderDistances() {
-    m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
-    var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
-
-    assertAll(
-        () -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon),
-        () -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon),
-        () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
-    );
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
deleted file mode 100644
index 75b6f44..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
+++ /dev/null
@@ -1,263 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-@SuppressWarnings("PMD.TooManyMethods")
-class MecanumDriveKinematicsTest {
-  private static final double kEpsilon = 1E-9;
-
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-  private final MecanumDriveKinematics m_kinematics =
-      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  @Test
-  void testStraightLineInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
-    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
-
-    /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
-      */
-
-    assertAll(
-        () -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testStraightLineForwardKinematicsKinematics() {
-
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
-      */
-
-    assertAll(
-        () -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testStrafeInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
-    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
-
-    /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
-      */
-
-    assertAll(
-        () -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testStrafeForwardKinematicsKinematics() {
-
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
-      */
-
-    assertAll(
-        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
-    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
-
-    /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
-      */
-
-    assertAll(
-        () -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testRotationForwardKinematicsKinematics() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should be [[0][0][2pi]]
-      */
-
-    assertAll(
-        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testMixedTranslationRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
-    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
-
-    /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
-      */
-
-    assertAll(
-        () -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testMixedTranslationRotationForwardKinematicsKinematics() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be [[2][3][1]]
-      */
-
-    assertAll(
-        () -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
-    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
-
-    /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
-      */
-
-    assertAll(
-        () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterRotationForwardKinematicsKinematics() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
-    velocities should be [[12][-12][1]]
-      */
-
-    assertAll(
-        () -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterTranslationRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
-    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
-
-    /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
-      */
-
-    assertAll(
-        () -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterRotationTranslationForwardKinematicsKinematics() {
-
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
-    velocities should be [[17][-10][1]]
-      */
-
-    assertAll(
-        () -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testNormalize() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
-    wheelSpeeds.normalize(5.5);
-
-    double factor = 5.5 / 7.0;
-
-    assertAll(
-        () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
-        () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
-        () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
-        () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)
-    );
-  }
-
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
deleted file mode 100644
index 4619bf0..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
+++ /dev/null
@@ -1,94 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class MecanumDriveOdometryTest {
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-
-  private final MecanumDriveKinematics m_kinematics =
-      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
-      new Rotation2d());
-
-  @Test
-  void testMultipleConsecutiveUpdates() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
-    var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(secondPose.getTranslation().getX(), 0.0, 0.01),
-        () -> assertEquals(secondPose.getTranslation().getY(), 0.0, 0.01),
-        () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)
-    );
-  }
-
-  @Test
-  void testTwoIterations() {
-    // 5 units/sec  in the x axis (forward)
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01),
-        () -> assertEquals(0, pose.getTranslation().getY(), 0.01),
-        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void test90degreeTurn() {
-    // This is a 90 degree turn about the point between front left and rear left wheels
-    // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01),
-        () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
-        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void testGyroAngleReset() {
-    var gyro = Rotation2d.fromDegrees(90.0);
-    var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
-        3.536, 3.536);
-    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
-
-    assertAll(
-        () -> assertEquals(5.0, pose.getTranslation().getX(), 0.1),
-        () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
-        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
-    );
-  }
-
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
deleted file mode 100644
index f506436..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
+++ /dev/null
@@ -1,263 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-@SuppressWarnings("PMD.TooManyMethods")
-class SwerveDriveKinematicsTest {
-  private static final double kEpsilon = 1E-9;
-
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-  private final SwerveDriveKinematics m_kinematics =
-      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  @Test
-  void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
-
-    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
-
-    assertAll(
-        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
-    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testStraightStrafeInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
-
-    assertAll(
-        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testStraightStrafeForwardKinematics() {
-    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testTurnInPlaceInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
-
-    /*
-    The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
-    the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
-    trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
-    our wheels must trace out 1 rotation (or 106.63 inches) per second.
-      */
-
-    assertAll(
-        () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
-        () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
-        () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
-        () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1),
-        () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testTurnInPlaceForwardKinematics() {
-    SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
-    SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
-    SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
-    SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
-
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterCORRotationInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
-
-    /*
-    This one is a bit trickier. Because we are rotating about the front-left wheel,
-    it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
-    an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
-    radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
-    should be pointing straight forward, the back-left wheel should be pointing straight right,
-    and the back-right wheel should be at a -45 degree angle
-    */
-
-    assertAll(
-        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1),
-        () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
-        () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
-        () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1),
-        () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testOffCenterCORRotationForwardKinematics() {
-    SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
-    SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
-    SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
-    SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
-
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
-
-    /*
-    We already know that our omega should be 2pi from the previous test. Next, we need to determine
-    the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
-    we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
-    a full revolution about the center of revolution once every second. Therefore, the center of
-    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
-    1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
-    */
-
-    assertAll(
-        () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
-        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual,
-                                 SwerveModuleState tolerance) {
-    assertAll(
-        () -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond,
-            tolerance.speedMetersPerSecond),
-        () -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(),
-            tolerance.angle.getDegrees())
-    );
-  }
-
-  /**
-   * Test the rotation of the robot about a non-central point with
-   * both linear and angular velocities.
-   */
-  @Test
-  void testOffCenterCORRotationAndTranslationInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
-
-    // By equation (13.14) from state-space guide, our wheels/angles will be as follows,
-    // (+-1 degree or speed):
-    SwerveModuleState[] expectedStates = new SwerveModuleState[]{
-        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
-        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
-        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
-        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
-    };
-    var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
-
-    for (int i = 0; i < expectedStates.length; i++) {
-      assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
-    }
-  }
-
-  @Test
-  void testOffCenterCORRotationAndTranslationForwardKinematics() {
-    SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
-    SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
-    SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
-    SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
-
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
-
-    /*
-    From equation (13.17), we know that chassis motion is th dot product of the
-    pseudoinverse of the inverseKinematics matrix (with the center of rotation at
-    (0,0) -- we don't want the motion of the center of rotation, we want it of
-    the center of the robot). These above SwerveModuleStates are known to be from
-    a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
-    calculated using Numpy's linalg.pinv function.
-    */
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testNormalize() {
-    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);
-
-    double factor = 5.5 / 7.0;
-
-    assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)
-    );
-  }
-
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
deleted file mode 100644
index 85b832c..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
+++ /dev/null
@@ -1,96 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class SwerveDriveOdometryTest {
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-  private final SwerveDriveKinematics m_kinematics =
-      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
-      new Rotation2d());
-
-  @Test
-  void testTwoIterations() {
-    // 5 units/sec  in the x axis (forward)
-    final SwerveModuleState[] wheelSpeeds = {
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0))
-    };
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(),
-        new SwerveModuleState(), new SwerveModuleState(),
-        new SwerveModuleState(), new SwerveModuleState());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01),
-        () -> assertEquals(0, pose.getTranslation().getY(), 0.01),
-        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void test90degreeTurn() {
-    // This is a 90 degree turn about the point between front left and rear left wheels
-    //        Module 0: speed 18.84955592153876 angle 90.0
-    //        Module 1: speed 42.14888838624436 angle 26.565051177077986
-    //        Module 2: speed 18.84955592153876 angle -90.0
-    //        Module 3: speed 42.14888838624436 angle -26.565051177077986
-
-    final SwerveModuleState[] wheelSpeeds = {
-        new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
-        new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
-        new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
-        new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
-    };
-    final var zero = new SwerveModuleState();
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
-    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01),
-        () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
-        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void testGyroAngleReset() {
-    var gyro = Rotation2d.fromDegrees(90.0);
-    var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var state = new SwerveModuleState();
-    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
-    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
-    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
-
-    assertAll(
-        () -> assertEquals(1.0, pose.getTranslation().getX(), 0.1),
-        () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
-        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
-    );
-  }
-
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java
deleted file mode 100644
index 91210bc..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogInputSimTest.java
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.sim;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.sim.AnalogInSim;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class AnalogInputSimTest {
-  static class DoubleStore {
-    public boolean m_wasTriggered;
-    public boolean m_wasCorrectType;
-    public double m_setValue0;
-  }
-
-  @Test
-  void setCallbackTest() {
-    HAL.initialize(500, 0);
-
-    AnalogInput input = new AnalogInput(5);
-    AnalogInSim inputSim = input.getSimObject();
-
-    for (double i = 0; i < 5.0; i += 0.1) {
-      inputSim.setVoltage(0);
-
-      assertEquals(input.getVoltage(), 0, 0.001);
-
-      inputSim.setVoltage(i);
-
-      assertEquals(input.getVoltage(), i, 0.001);
-
-    }
-
-    input.close();
-
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java
deleted file mode 100644
index 7771425..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/sim/AnalogOutputSimTest.java
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.sim;
-
-import edu.wpi.first.wpilibj.AnalogOutput;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.sim.AnalogOutSim;
-import edu.wpi.first.hal.sim.CallbackStore;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-import org.junit.jupiter.api.Test;
-
-class AnalogOutputSimTest {
-  static class DoubleStore {
-    public boolean m_wasTriggered;
-    public boolean m_wasCorrectType;
-    public double m_setValue = -1;
-
-    public void reset() {
-      m_wasCorrectType = false;
-      m_wasTriggered = false;
-      m_setValue = -1;
-    }
-  }
-
-  @Test
-  void setCallbackTest() {
-    HAL.initialize(500, 0);
-
-
-    AnalogOutput output = new AnalogOutput(0);
-    output.setVoltage(0.5);
-
-    AnalogOutSim outputSim = output.getSimObject();
-
-    DoubleStore store = new DoubleStore();
-
-    try (CallbackStore cb = outputSim.registerVoltageCallback((name, value) -> {
-      store.m_wasTriggered = true;
-      store.m_wasCorrectType = true;
-      store.m_setValue = value.getDouble();
-    }, false)) {
-      assertFalse(store.m_wasTriggered);
-
-      for (double i = 0.1; i < 5.0; i += 0.1) {
-        store.reset();
-
-        output.setVoltage(0);
-
-        assertTrue(store.m_wasTriggered);
-        assertEquals(store.m_setValue, 0, 0.001);
-
-        store.reset();
-
-        output.setVoltage(i);
-
-        assertTrue(store.m_wasTriggered);
-        assertEquals(store.m_setValue, i, 0.001);
-
-      }
-    }
-
-    output.close();
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java
new file mode 100644
index 0000000..05ea290
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.AccelerometerJNI;
+import edu.wpi.first.hal.HAL;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class AccelerometerSimTest {
+  static class TriggeredStore {
+    public boolean m_wasTriggered;
+    public boolean m_setValue = true;
+  }
+
+  @Test
+  void testCallbacks() {
+    HAL.initialize(500, 0);
+    BuiltInAccelerometerSim sim = new BuiltInAccelerometerSim();
+    sim.resetData();
+
+    TriggeredStore store = new TriggeredStore();
+
+    try (CallbackStore cb = sim.registerActiveCallback((s, v) -> {
+      store.m_wasTriggered = true;
+      store.m_setValue = v.getBoolean();
+    }, false)) {
+      assertFalse(store.m_wasTriggered);
+      AccelerometerJNI.setAccelerometerActive(true);
+      assertTrue(store.m_wasTriggered);
+      assertTrue(store.m_setValue);
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
new file mode 100644
index 0000000..677c5eb
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class AnalogEncoderSimTest {
+  @Test
+  public void testBasic() {
+    var analogInput = new AnalogInput(0);
+    var analogEncoder = new AnalogEncoder(analogInput);
+    var encoderSim = new AnalogEncoderSim(analogEncoder);
+
+    encoderSim.setPosition(Rotation2d.fromDegrees(180));
+    assertEquals(analogEncoder.get(), 0.5, 1E-8);
+    assertEquals(encoderSim.getTurns(), 0.5, 1E-8);
+    assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java
new file mode 100644
index 0000000..a707725
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.hal.HAL;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class AnalogInputSimTest {
+  static class DoubleStore {
+    public boolean m_wasTriggered;
+    public boolean m_wasCorrectType;
+    public double m_setValue0;
+  }
+
+  @Test
+  void setCallbackTest() {
+    HAL.initialize(500, 0);
+
+    try (AnalogInput input = new AnalogInput(5)) {
+      AnalogInputSim inputSim = new AnalogInputSim(input);
+
+      for (double i = 0; i < 5.0; i += 0.1) {
+        inputSim.setVoltage(0);
+
+        assertEquals(input.getVoltage(), 0, 0.001);
+
+        inputSim.setVoltage(i);
+
+        assertEquals(input.getVoltage(), i, 0.001);
+      }
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java
new file mode 100644
index 0000000..fe1bba8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.AnalogOutput;
+import edu.wpi.first.hal.HAL;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class AnalogOutputSimTest {
+  static class DoubleStore {
+    public boolean m_wasTriggered;
+    public boolean m_wasCorrectType;
+    public double m_setValue = -1;
+
+    public void reset() {
+      m_wasCorrectType = false;
+      m_wasTriggered = false;
+      m_setValue = -1;
+    }
+  }
+
+  @Test
+  void setCallbackTest() {
+    HAL.initialize(500, 0);
+
+
+    try (AnalogOutput output = new AnalogOutput(0)) {
+      output.setVoltage(0.5);
+
+      AnalogOutputSim outputSim = new AnalogOutputSim(output);
+
+      DoubleStore store = new DoubleStore();
+
+      try (CallbackStore cb = outputSim.registerVoltageCallback((name, value) -> {
+        store.m_wasTriggered = true;
+        store.m_wasCorrectType = true;
+        store.m_setValue = value.getDouble();
+      }, false)) {
+        assertFalse(store.m_wasTriggered);
+
+        for (double i = 0.1; i < 5.0; i += 0.1) {
+          store.reset();
+
+          output.setVoltage(0);
+
+          assertTrue(store.m_wasTriggered);
+          assertEquals(store.m_setValue, 0, 0.001);
+
+          store.reset();
+
+          output.setVoltage(i);
+
+          assertTrue(store.m_wasTriggered);
+          assertEquals(store.m_setValue, i, 0.001);
+        }
+      }
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
new file mode 100644
index 0000000..4a0eab3
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.Vector;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N7;
+
+import org.junit.jupiter.api.Test;
+
+import java.util.List;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDrivetrainSimTest {
+  @Test
+  public void testConvergence() {
+    var motor = DCMotor.getNEO(2);
+    var plant = LinearSystemId.createDrivetrainVelocitySystem(
+        motor,
+        50,
+        Units.inchesToMeters(2),
+        Units.inchesToMeters(12),
+        0.5,
+        1.0);
+
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim = new DifferentialDrivetrainSim(plant,
+        motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020);
+    var ramsete = new RamseteController();
+    feedforward.reset(VecBuilder.fill(0, 0));
+
+    // ground truth
+    Matrix<N7, N1> groundTruthX = new Vector<>(Nat.N7());
+
+    var traj = TrajectoryGenerator.generateTrajectory(
+        new Pose2d(),
+        List.of(),
+        new Pose2d(2, 2, new Rotation2d()),
+        new TrajectoryConfig(1, 1)
+            .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
+
+    for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) {
+      var state = traj.sample(0.020);
+      var ramseteOut = ramsete.calculate(sim.getPose(), state);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut);
+
+      var voltages = feedforward.calculate(
+          VecBuilder.fill(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond));
+
+      // Sim periodic code
+      sim.setInputs(voltages.get(0, 0), voltages.get(1, 0));
+      sim.update(0.020);
+
+      // Update our ground truth
+      groundTruthX = RungeKutta.rungeKutta(sim::getDynamics, groundTruthX, voltages, 0.020);
+    }
+
+    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kX));
+    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kY.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kY));
+    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kHeading.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kHeading));
+  }
+
+  @Test
+  public void testCurrent() {
+    var motor = DCMotor.getNEO(2);
+    var plant = LinearSystemId.createDrivetrainVelocitySystem(
+        motor,
+        50,
+        Units.inchesToMeters(2),
+        Units.inchesToMeters(12),
+        0.5,
+        1.0);
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+
+    sim.setInputs(-12, -12);
+    for (int i = 0; i < 10; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDrawAmps() > 0);
+
+    sim.setInputs(12, 12);
+    for (int i = 0; i < 20; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDrawAmps() > 0);
+
+    sim.setInputs(-12, 12);
+    for (int i = 0; i < 30; i++) {
+      sim.update(0.020);
+    }
+    assertTrue(sim.getCurrentDrawAmps() > 0);
+  }
+
+  @Test
+  public void testModelStability() {
+    var motor = DCMotor.getNEO(2);
+    var plant = LinearSystemId.createDrivetrainVelocitySystem(
+        motor,
+        50,
+        Units.inchesToMeters(2),
+        Units.inchesToMeters(12),
+        2.0,
+        5.0);
+
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
+    var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+
+    sim.setInputs(2, 4);
+
+    // 10 seconds should be enough time to verify stability
+    for (int i = 0; i < 500; i++) {
+      sim.update(0.020);
+    }
+
+    assertTrue(Math.abs(sim.getPose().getTranslation().getNorm()) < 100);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
new file mode 100644
index 0000000..c4d78d0
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class ElevatorSimTest {
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testStateSpaceSimWithElevator() {
+
+    var controller = new PIDController(10, 0, 0);
+
+    var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 14.67, 8, 0.75 * 25.4 / 1000.0,
+        0.0, 3.0, VecBuilder.fill(0.01));
+
+    var motor = new PWMVictorSPX(0);
+    var encoder = new Encoder(0, 1);
+    var encoderSim = new EncoderSim(encoder);
+
+    for (int i = 0; i < 100; i++) {
+      controller.setSetpoint(2.0);
+
+      double nextVoltage = controller.calculate(encoderSim.getDistance());
+
+      double currentBatteryVoltage = RobotController.getBatteryVoltage();
+      motor.set(nextVoltage / currentBatteryVoltage);
+
+      // ------ SimulationPeriodic() happens after user code -------
+
+      var u = VecBuilder.fill(motor.get() * currentBatteryVoltage);
+      sim.setInput(u);
+      sim.update(0.020);
+      var y = sim.getOutput();
+      encoderSim.setDistance(y.get(0, 0));
+    }
+
+    assertEquals(controller.getSetpoint(), sim.getPositionMeters(), 0.2);
+  }
+
+  @Test
+  public void testMinMax() {
+    var plant = LinearSystemId.createElevatorSystem(
+        DCMotor.getVex775Pro(4),
+        8.0,
+        0.75 * 25.4 / 1000.0,
+        14.67);
+
+    var sim = new ElevatorSim(plant,
+        DCMotor.getVex775Pro(4),
+        14.67, 0.75 * 25.4 / 1000.0, 0.0, 1.0, VecBuilder.fill(0.01));
+
+    for (int i = 0; i < 100; i++) {
+      sim.setInput(VecBuilder.fill(0));
+      sim.update(0.020);
+      var height = sim.getPositionMeters();
+      assertTrue(height >= -0.05);
+    }
+
+    for (int i = 0; i < 100; i++) {
+      sim.setInput(VecBuilder.fill(12.0));
+      sim.update(0.020);
+      var height = sim.getPositionMeters();
+      assertTrue(height <= 1.05);
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
new file mode 100644
index 0000000..6b9fe61
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class SimDeviceSimTest {
+  @Test
+  void testBasic() {
+    SimDevice dev = SimDevice.create("test");
+    SimBoolean devBool = dev.createBoolean("bool", false, false);
+
+    SimDeviceSim sim = new SimDeviceSim("test");
+    SimBoolean simBool = sim.getBoolean("bool");
+
+    assertFalse(simBool.get());
+    simBool.set(true);
+    assertTrue(devBool.get());
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
new file mode 100644
index 0000000..f1b4bc8
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class SingleJointedArmSimTest {
+  SingleJointedArmSim m_sim = new SingleJointedArmSim(
+      DCMotor.getVex775Pro(2),
+      100,
+      3.0,
+      Units.inchesToMeters(19.0 / 2.0),
+      -Math.PI,
+      0.0, 10.0 / 2.2, true);
+
+  @Test
+  public void testArmDisabled() {
+    m_sim.setState(VecBuilder.fill(0.0, 0.0));
+
+    for (int i = 0; i < 12 / 0.02; i++) {
+      m_sim.setInput(0.0);
+      m_sim.update(0.020);
+    }
+
+    // the arm should swing down
+    assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1);
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
deleted file mode 100644
index 45a0638..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
+++ /dev/null
@@ -1,162 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-
-class CubicHermiteSplineTest {
-  private static final double kMaxDx = 0.127;
-  private static final double kMaxDy = 0.00127;
-  private static final double kMaxDtheta = 0.0872;
-
-  @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
-  private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
-    // Start the timer.
-    var start = System.nanoTime();
-
-    // Generate and parameterize the spline.
-    var controlVectors =
-        SplineHelper.getCubicControlVectorsFromWaypoints(a,
-            waypoints.toArray(new Translation2d[0]), b);
-    var splines
-        = SplineHelper.getCubicSplinesFromControlVectors(
-        controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
-
-    var poses = new ArrayList<PoseWithCurvature>();
-
-    poses.add(splines[0].getPoint(0.0));
-
-    for (var spline : splines) {
-      poses.addAll(SplineParameterizer.parameterize(spline));
-    }
-
-    // End the timer.
-    var end = System.nanoTime();
-
-    // Calculate the duration (used when benchmarking)
-    var durationMicroseconds = (end - start) / 1000.0;
-
-    for (int i = 0; i < poses.size() - 1; i++) {
-      var p0 = poses.get(i);
-      var p1 = poses.get(i + 1);
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      var twist = p0.poseMeters.log(p1.poseMeters);
-      assertAll(
-          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
-          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
-          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
-      );
-    }
-
-    // Check first point
-    assertAll(
-        () -> assertEquals(a.getTranslation().getX(),
-            poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(a.getTranslation().getY(),
-            poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
-        () -> assertEquals(a.getRotation().getRadians(),
-            poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
-    );
-
-    // Check interior waypoints
-    boolean interiorsGood = true;
-    for (var waypoint : waypoints) {
-      boolean found = false;
-      for (var state : poses) {
-        if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
-          found = true;
-        }
-      }
-      interiorsGood &= found;
-    }
-
-    assertTrue(interiorsGood);
-
-    // Check last point
-    assertAll(
-        () -> assertEquals(b.getTranslation().getX(),
-            poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(b.getTranslation().getY(),
-            poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9),
-        () -> assertEquals(b.getRotation().getRadians(),
-            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
-    );
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testStraightLine() {
-    run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testSCurve() {
-    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
-    ArrayList<Translation2d> waypoints = new ArrayList<>();
-    waypoints.add(new Translation2d(1, 1));
-    waypoints.add(new Translation2d(2, -1));
-    var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
-
-    run(start, waypoints, end);
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testOneInterior() {
-    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
-    ArrayList<Translation2d> waypoints = new ArrayList<>();
-    waypoints.add(new Translation2d(2.0, 0.0));
-    var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
-
-    run(start, waypoints, end);
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testWindyPath() {
-    final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
-    final ArrayList<Translation2d> waypoints = new ArrayList<>();
-    waypoints.add(new Translation2d(0.5, 0.5));
-    waypoints.add(new Translation2d(0.5, 0.5));
-    waypoints.add(new Translation2d(1.0, 0.0));
-    waypoints.add(new Translation2d(1.5, 0.5));
-    waypoints.add(new Translation2d(2.0, 0.0));
-    waypoints.add(new Translation2d(2.5, 0.5));
-    final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
-
-    run(start, waypoints, end);
-  }
-
-  @Test
-  void testMalformed() {
-    assertThrows(MalformedSplineException.class, () -> run(
-        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-        new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
-    assertThrows(MalformedSplineException.class, () -> run(
-        new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
-        Arrays.asList(new Translation2d(10, 10.5)),
-        new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
deleted file mode 100644
index 1d65922..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
+++ /dev/null
@@ -1,108 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class QuinticHermiteSplineTest {
-  private static final double kMaxDx = 0.127;
-  private static final double kMaxDy = 0.00127;
-  private static final double kMaxDtheta = 0.0872;
-
-  @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
-  private void run(Pose2d a, Pose2d b) {
-    // Start the timer.
-    var start = System.nanoTime();
-
-    // Generate and parameterize the spline.
-    var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b));
-    var spline = SplineHelper.getQuinticSplinesFromControlVectors(
-        controlVectors.toArray(new Spline.ControlVector[0]))[0];
-    var poses = SplineParameterizer.parameterize(spline);
-
-    // End the timer.
-    var end = System.nanoTime();
-
-    // Calculate the duration (used when benchmarking)
-    var durationMicroseconds = (end - start) / 1000.0;
-
-    for (int i = 0; i < poses.size() - 1; i++) {
-      var p0 = poses.get(i);
-      var p1 = poses.get(i + 1);
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      var twist = p0.poseMeters.log(p1.poseMeters);
-      assertAll(
-          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
-          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
-          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
-    }
-
-    // Check first point
-    assertAll(
-        () -> assertEquals(
-            a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(
-            a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9),
-        () -> assertEquals(
-            a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
-            1E-9));
-
-    // Check last point
-    assertAll(
-        () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1)
-            .poseMeters.getTranslation().getX(), 1E-9),
-        () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1)
-            .poseMeters.getTranslation().getY(), 1E-9),
-        () -> assertEquals(b.getRotation().getRadians(),
-            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testStraightLine() {
-    run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testSimpleSCurve() {
-    run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testSquiggly() {
-    run(
-        new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
-        new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
-  }
-
-  @Test
-  void testMalformed() {
-    assertThrows(MalformedSplineException.class,
-        () -> run(
-          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-          new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
-    assertThrows(MalformedSplineException.class,
-        () -> run(
-          new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
-          new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
deleted file mode 100644
index 977c5ab..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.Collections;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint;
-import edu.wpi.first.wpilibj.util.Units;
-
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class CentripetalAccelerationConstraintTest {
-  @SuppressWarnings("LocalVariableName")
-  @Test
-  void testCentripetalAccelerationConstraint() {
-    double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
-    var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
-
-    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
-        Collections.singletonList(constraint));
-
-    var duration = trajectory.getTotalTimeSeconds();
-    var t = 0.0;
-    var dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      var centripetalAcceleration
-          = Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
-
-      t += dt;
-      assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
-    }
-  }
-
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
deleted file mode 100644
index d025860..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
+++ /dev/null
@@ -1,53 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.Collections;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.util.Units;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class DifferentialDriveKinematicsConstraintTest {
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  @Test
-  void testDifferentialDriveKinematicsConstraint() {
-    double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
-    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
-    var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
-
-    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
-        Collections.singletonList(constraint));
-
-    var duration = trajectory.getTotalTimeSeconds();
-    var t = 0.0;
-    var dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      var chassisSpeeds = new ChassisSpeeds(
-          point.velocityMetersPerSecond, 0,
-          point.velocityMetersPerSecond * point.curvatureRadPerMeter
-      );
-
-      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
-
-      t += dt;
-      assertAll(
-          () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
-          () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)
-      );
-    }
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
deleted file mode 100644
index f5c6ff3..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.Collections;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class DifferentialDriveVoltageConstraintTest {
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  @Test
-  void testDifferentialDriveVoltageConstraint() {
-    // Pick an unreasonably large kA to ensure the constraint has to do some work
-    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
-    var kinematics = new DifferentialDriveKinematics(0.5);
-    double maxVoltage = 10;
-    var constraint = new DifferentialDriveVoltageConstraint(feedforward,
-                                                            kinematics,
-                                                            maxVoltage);
-
-    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
-        Collections.singletonList(constraint));
-
-    var duration = trajectory.getTotalTimeSeconds();
-    var t = 0.0;
-    var dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      var chassisSpeeds = new ChassisSpeeds(
-          point.velocityMetersPerSecond, 0,
-          point.velocityMetersPerSecond * point.curvatureRadPerMeter
-      );
-
-      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
-
-      t += dt;
-
-      // Not really a strictly-correct test as we're using the chassis accel instead of the
-      // wheel accel, but much easier than doing it "properly" and a reasonable check anyway
-      assertAll(
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               <= maxVoltage + 0.05),
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               >= -maxVoltage - 0.05),
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               <= maxVoltage + 0.05),
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               >= -maxVoltage - 0.05)
-      );
-    }
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
deleted file mode 100644
index 81fb5b4..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
+++ /dev/null
@@ -1,95 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.hal.sim.DriverStationSim;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
-
-import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class TrajectoryGeneratorTest {
-  static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
-    final double maxVelocity = feetToMeters(12.0);
-    final double maxAccel = feetToMeters(12);
-
-    // 2018 cross scale auto waypoints.
-    var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23),
-        Rotation2d.fromDegrees(-180));
-    var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8),
-        Rotation2d.fromDegrees(-160));
-
-    var waypoints = new ArrayList<Pose2d>();
-    waypoints.add(sideStart);
-    waypoints.add(sideStart.plus(
-        new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)),
-            new Rotation2d())));
-    waypoints.add(sideStart.plus(
-        new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
-            Rotation2d.fromDegrees(-90))));
-    waypoints.add(crossScale);
-
-    TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
-        .setReversed(true)
-        .addConstraints(constraints);
-
-    return TrajectoryGenerator.generateTrajectory(waypoints, config);
-  }
-
-  @Test
-  @SuppressWarnings("LocalVariableName")
-  void testGenerationAndConstraints() {
-    Trajectory trajectory = getTrajectory(new ArrayList<>());
-
-    double duration = trajectory.getTotalTimeSeconds();
-    double t = 0.0;
-    double dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      t += dt;
-      assertAll(
-          () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
-          () -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0)
-              + 0.05)
-      );
-    }
-  }
-
-  @Test
-  void testMalformedTrajectory() {
-    var dsSim = new DriverStationSim();
-    dsSim.setSendError(false);
-
-    var traj =
-        TrajectoryGenerator.generateTrajectory(
-          Arrays.asList(
-            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-            new Pose2d(1, 0, Rotation2d.fromDegrees(180))
-          ),
-          new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
-        );
-
-    assertEquals(traj.getStates().size(), 1);
-    assertEquals(traj.getTotalTimeSeconds(), 0);
-
-    dsSim.setSendError(true);
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
deleted file mode 100644
index e389a4f..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-public class TrajectoryJsonTest {
-  @Test
-  void deserializeMatches() {
-    var config = List.of(new DifferentialDriveKinematicsConstraint(
-        new DifferentialDriveKinematics(20), 3));
-    var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
-
-    var deserialized =
-        assertDoesNotThrow(() ->
-            TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory)));
-
-    assertEquals(trajectory.getStates(), deserialized.getStates());
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
deleted file mode 100644
index 19f31dc..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class TrajectoryTransformTest {
-  @Test
-  void testTransformBy() {
-    var config = new TrajectoryConfig(3, 3);
-    var trajectory = TrajectoryGenerator.generateTrajectory(
-        new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
-        config
-    );
-
-    var transformedTrajectory = trajectory.transformBy(
-        new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
-
-    // Test initial pose.
-    assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
-        transformedTrajectory.sample(0).poseMeters);
-
-    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
-  }
-
-  @Test
-  void testRelativeTo() {
-    var config = new TrajectoryConfig(3, 3);
-    var trajectory = TrajectoryGenerator.generateTrajectory(
-        new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
-        List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
-        config
-    );
-
-    var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
-
-    // Test initial pose.
-    assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
-
-    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
-  }
-
-  void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
-    for (int i = 0; i < statesA.size() - 1; i++) {
-      var a1 = statesA.get(i).poseMeters;
-      var a2 = statesA.get(i + 1).poseMeters;
-
-      var b1 = statesB.get(i).poseMeters;
-      var b2 = statesB.get(i + 1).poseMeters;
-
-      assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
-    }
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
deleted file mode 100644
index 090eacc..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
+++ /dev/null
@@ -1,257 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-@SuppressWarnings({"PMD.TooManyMethods", "PMD.AvoidInstantiatingObjectsInLoops"})
-class TrapezoidProfileTest {
-  private static final double kDt = 0.01;
-
-  /**
-   * Asserts "val1" is less than or equal to "val2".
-   *
-   * @param val1 First operand in comparison.
-   * @param val2 Second operand in comparison.
-   */
-  private static void assertLessThanOrEquals(double val1, double val2) {
-    assertTrue(val1 <= val2, val1 + " is greater than " + val2);
-  }
-
-  /**
-   * Asserts "val1" is within "eps" of "val2".
-   *
-   * @param val1 First operand in comparison.
-   * @param val2 Second operand in comparison.
-   * @param eps Tolerance for whether values are near to each other.
-   */
-  private static void assertNear(double val1, double val2, double eps) {
-    assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
-        + " is greater than " + eps);
-  }
-
-  /**
-   * Asserts "val1" is less than or within "eps" of "val2".
-   *
-   * @param val1 First operand in comparison.
-   * @param val2 Second operand in comparison.
-   * @param eps Tolerance for whether values are near to each other.
-   */
-  private static void assertLessThanOrNear(double val1, double val2, double eps) {
-    if (val1 <= val2) {
-      assertLessThanOrEquals(val1, val2);
-    } else {
-      assertNear(val1, val2, eps);
-    }
-  }
-
-  @Test
-  void reachesGoal() {
-    TrapezoidProfile.Constraints constraints =
-        new TrapezoidProfile.Constraints(1.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 450; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(state, goal);
-  }
-
-  // Tests that decreasing the maximum velocity in the middle when it is already
-  // moving faster than the new max is handled correctly
-  @Test
-  void posContinousUnderVelChange() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double lastPos = state.position;
-    for (int i = 0; i < 1600; ++i) {
-      if (i == 400) {
-        constraints.maxVelocity = 0.75;
-      }
-
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      double estimatedVel = (state.position - lastPos) / kDt;
-
-      if (i >= 400) {
-        // Since estimatedVel can have floating point rounding errors, we check
-        // whether value is less than or within an error delta of the new
-        // constraint.
-        assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
-
-        assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
-      }
-
-      lastPos = state.position;
-    }
-    assertEquals(state, goal);
-  }
-
-  // There is some somewhat tricky code for dealing with going backwards
-  @Test
-  void backwards() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 400; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(state, goal);
-  }
-
-  @Test
-  void switchGoalInMiddle() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertNotEquals(state, goal);
-
-    goal = new TrapezoidProfile.State(0.0, 0.0);
-    for (int i = 0; i < 550; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(state, goal);
-  }
-
-  // Checks to make sure that it hits top speed
-  @Test
-  void topSpeed() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertNear(constraints.maxVelocity, state.velocity, 10e-5);
-
-    for (int i = 0; i < 2000; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(state, goal);
-  }
-
-  @Test
-  void timingToCurrent() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 400; i++) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
-    }
-  }
-
-  @Test
-  void timingToGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
-    boolean reachedGoal = false;
-    for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      if (!reachedGoal && state.equals(goal)) {
-        // Expected value using for loop index is just an approximation since
-        // the time left in the profile doesn't increase linearly at the
-        // endpoints
-        assertNear(predictedTimeLeft, i / 100.0, 0.25);
-        reachedGoal = true;
-      }
-    }
-  }
-
-  @Test
-  void timingBeforeGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(1);
-    boolean reachedGoal = false;
-    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)) {
-        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
-        reachedGoal = true;
-      }
-    }
-  }
-
-  @Test
-  void timingToNegativeGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
-    boolean reachedGoal = false;
-    for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      if (!reachedGoal && state.equals(goal)) {
-        // Expected value using for loop index is just an approximation since
-        // the time left in the profile doesn't increase linearly at the
-        // endpoints
-        assertNear(predictedTimeLeft, i / 100.0, 0.25);
-        reachedGoal = true;
-      }
-    }
-  }
-
-  @Test
-  void timingBeforeNegativeGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(-1);
-    boolean reachedGoal = false;
-    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)) {
-        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
-        reachedGoal = true;
-      }
-    }
-  }
-}

wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java
new file mode 100644
index 0000000..231eab6
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import java.util.stream.Stream;
+
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class ColorTest {
+  private static final double kEpsilon = 1e-3;
+
+  void assertColorMatches(double red, double green, double blue, Color color) {
+    assertAll(
+        () -> assertEquals(red, color.red, kEpsilon),
+        () -> assertEquals(green, color.green, kEpsilon),
+        () -> assertEquals(blue, color.blue, kEpsilon)
+    );
+  }
+
+  @ParameterizedTest
+  @MethodSource("staticColorProvider")
+  void staticColorTest(double red, double green, double blue, Color color) {
+    assertColorMatches(red, green, blue, color);
+  }
+
+  @ParameterizedTest
+  @MethodSource("staticColorProvider")
+  void colorEqualsTest(double red, double green, double blue, Color color) {
+    assertEquals(color, new Color(red, green, blue));
+  }
+
+  static Stream<Arguments> staticColorProvider() {
+    return Stream.of(
+      arguments(0.0823529412, 0.376470589, 0.7411764706, Color.kDenim),
+      arguments(0.0, 0.4, 0.7019607844, Color.kFirstBlue),
+      arguments(0.9294117648, 0.1098039216, 0.1411764706, Color.kFirstRed)
+    );
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
deleted file mode 100644
index 1d00046..0000000
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.util;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.UtilityClassTest;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class UnitsTest extends UtilityClassTest<Units> {
-  UnitsTest() {
-    super(Units.class);
-  }
-
-  @Test
-  void metersToFeetTest() {
-    assertEquals(3.28, Units.metersToFeet(1), 1e-2);
-  }
-
-  @Test
-  void feetToMetersTest() {
-    assertEquals(0.30, Units.feetToMeters(1), 1e-2);
-  }
-
-  @Test
-  void metersToInchesTest() {
-    assertEquals(39.37, Units.metersToInches(1), 1e-2);
-  }
-
-  @Test
-  void inchesToMetersTest() {
-    assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
-  }
-
-  @Test
-  void degreesToRadiansTest() {
-    assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
-  }
-
-  @Test
-  void radiansToDegreesTest() {
-    assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
-  }
-
-  @Test
-  void rotationsPerMinuteToRadiansPerSecondTest() {
-    assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
-  }
-
-  @Test
-  void radiansPerSecondToRotationsPerMinute() {
-    assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
-  }
-}

wpilibjExamples/build.gradle

diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle
index 2016632..73185ba 100644
--- a/wpilibjExamples/build.gradle
+++ b/wpilibjExamples/build.gradle
@@ -13,6 +13,7 @@
 dependencies {
     implementation project(':wpilibj')
 
+    implementation project(':wpimath')
     implementation project(':hal')
     implementation project(':wpiutil')
     implementation project(':ntcore')

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
index 6764ed7..4a8b0fc 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeInstantCommand extends InstantCommand {
   public ReplaceMeInstantCommand() {
     // Use addRequirements() here to declare subsystem dependencies.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
index f293c41..92958b3 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeParallelCommandGroup extends ParallelCommandGroup {
   /**
    * Creates a new ReplaceMeParallelCommandGroup.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
index 9092834..82c000e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeParallelDeadlineGroup extends ParallelDeadlineGroup {
   /**
    * Creates a new ReplaceMeParallelDeadlineGroup.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
index 686f339..f710ee4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeParallelRaceGroup extends ParallelRaceGroup {
   /**
    * Creates a new ReplaceMeParallelRaceGroup.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
index 2615b08..db1584b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMePIDCommand extends PIDCommand {
   /**
    * Creates a new ReplaceMePIDCommand.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
index 91a6e82..1fc4c2d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeProfiledPIDCommand extends ProfiledPIDCommand {
   /**
    * Creates a new ReplaceMeProfiledPIDCommand.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
index f08e2dd..1038b04 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,7 +11,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeSequentialCommandGroup extends SequentialCommandGroup {
   /**
    * Creates a new ReplaceMeSequentialCommandGroup.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
index d779a22..646817f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
-// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand {
   /**
    * Creates a new ReplaceMeTrapezoidProfileCommand.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
index a171fc6..8c9cef6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -54,6 +54,7 @@
    */
   @Override
   public void disabledInit() {
+    m_robotContainer.disablePIDSubsystems();
   }
 
   @Override

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
index 5558f25..afe7317 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -62,11 +62,21 @@
 
     // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
     new JoystickButton(m_driverController, Button.kA.value)
-        .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
+        .whenPressed(() -> {
+          m_robotArm.setGoal(2);
+          m_robotArm.enable();
+        }, m_robotArm);
 
     // Move the arm to neutral position when the 'B' button is pressed.
     new JoystickButton(m_driverController, Button.kB.value)
-        .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
+        .whenPressed(() -> {
+          m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads);
+          m_robotArm.enable();
+        }, m_robotArm);
+
+    // Disable the arm controller when Y is pressed.
+    new JoystickButton(m_driverController, Button.kY.value)
+        .whenPressed(m_robotArm::disable);
 
     // Drive at half speed when the bumper is held
     new JoystickButton(m_driverController, Button.kBumperRight.value)
@@ -74,6 +84,13 @@
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
   }
 
+  /**
+   * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This
+   * should be called on robot disable to prevent integral windup.
+   */
+  public void disablePIDSubsystems() {
+    m_robotArm.disable();
+  }
 
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
index 0fa7dae..153a254 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -53,9 +53,9 @@
   /**
    * Places this motor controller in follower mode.
    *
-   * @param master The master to follow.
+   * @param leader The leader to follow.
    */
-  public void follow(ExampleSmartMotorController master) {
+  public void follow(ExampleSmartMotorController leader) {
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
new file mode 100644
index 0000000..31777a6
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armsimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
new file mode 100644
index 0000000..c906381
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.armsimulation;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+/**
+ * This is a sample program to demonstrate the use of elevator simulation with existing code.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+  private static final int kJoystickPort = 0;
+
+  // The P gain for the PID controller that drives this arm.
+  private static final double kArmKp = 5.0;
+
+  // distance per pulse = (angle per revolution) / (pulses per revolution)
+  //  = (2 * PI rads) / (4096 pulses)
+  private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
+
+  // The arm gearbox represents a gerbox containing two Vex 775pro motors.
+  private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
+
+  // Standard classes for controlling our elevator
+  private final PIDController m_controller = new PIDController(kArmKp, 0, 0);
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+  private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  private static final double m_armReduction = 100;
+  private static final double m_armMass = 5.0; // Kilograms
+  private static final double m_armLength = Units.inchesToMeters(30);
+  // This arm sim represents an arm that can travel from -180 degrees (rotated straight backwards)
+  // to 0 degrees (rotated straight forwards).
+  private final SingleJointedArmSim m_armSim = new SingleJointedArmSim(m_armGearbox,
+      m_armReduction,
+      SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
+      m_armLength,
+      Units.degreesToRadians(-180),
+      Units.degreesToRadians(0),
+      m_armMass,
+      true,
+      VecBuilder.fill(Units.degreesToRadians(0.5)) // Add noise with a std-dev of 0.5 degrees
+      );
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+  @Override
+  public void robotInit() {
+    m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse);
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_armSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_armSim.getAngleRads());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getTrigger()) {
+      // Here, we run PID control like normal, with a constant setpoint of 30in.
+      var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(0));
+      m_motor.setVoltage(pidOutput);
+    } else {
+      // Otherwise, we disable the motor.
+      m_motor.set(0.0);
+    }
+  }
+
+  @Override
+  public void disabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_motor.set(0.0);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
index cc3d2e1..b9c69a8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,7 @@
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
@@ -30,18 +30,18 @@
   private static final double kWheelRadius = 0.0508; // meters
   private static final int kEncoderResolution = 4096;
 
-  private final SpeedController m_leftMaster = new PWMVictorSPX(1);
+  private final SpeedController m_leftLeader = new PWMVictorSPX(1);
   private final SpeedController m_leftFollower = new PWMVictorSPX(2);
-  private final SpeedController m_rightMaster = new PWMVictorSPX(3);
+  private final SpeedController m_rightLeader = new PWMVictorSPX(3);
   private final SpeedController m_rightFollower = new PWMVictorSPX(4);
 
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
   private final SpeedControllerGroup m_leftGroup
-      = new SpeedControllerGroup(m_leftMaster, m_leftFollower);
+      = new SpeedControllerGroup(m_leftLeader, m_leftFollower);
   private final SpeedControllerGroup m_rightGroup
-      = new SpeedControllerGroup(m_rightMaster, m_rightFollower);
+      = new SpeedControllerGroup(m_rightLeader, m_rightFollower);
 
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
@@ -53,6 +53,9 @@
 
   private final DifferentialDriveOdometry m_odometry;
 
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
   /**
    * Constructs a differential drive object.
    * Sets the encoder distance per pulse and resets the gyro.
@@ -69,17 +72,7 @@
     m_leftEncoder.reset();
     m_rightEncoder.reset();
 
-    m_odometry = new DifferentialDriveOdometry(getAngle());
-  }
-
-  /**
-   * Returns the angle of the robot as a Rotation2d.
-   *
-   * @return The angle of the robot.
-   */
-  public Rotation2d getAngle() {
-    // Negating the angle because WPILib gyros are CW positive.
-    return Rotation2d.fromDegrees(-m_gyro.getAngle());
+    m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
   }
 
   /**
@@ -88,12 +81,15 @@
    * @param speeds The desired wheel speeds.
    */
   public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
-    double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
+    final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+    final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+
+    final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
         speeds.leftMetersPerSecond);
-    double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
+    final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
         speeds.rightMetersPerSecond);
-    m_leftGroup.set(leftOutput);
-    m_rightGroup.set(rightOutput);
+    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
+    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
   }
 
   /**
@@ -112,6 +108,7 @@
    * Updates the field-relative position.
    */
   public void updateOdometry() {
-    m_odometry.update(getAngle(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
+        m_rightEncoder.getDistance());
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
index f7ff493..6e7416d 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.examples.differentialdrivebot;
 
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
@@ -15,6 +16,11 @@
   private final XboxController m_controller = new XboxController(0);
   private final Drivetrain m_drive = new Drivetrain();
 
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+
   @Override
   public void autonomousPeriodic() {
     teleopPeriodic();
@@ -25,13 +31,17 @@
   public void teleopPeriodic() {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
+    final var xSpeed =
+        -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
+            * Drivetrain.kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed;
+    final var rot =
+        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
+            * Drivetrain.kMaxAngularSpeed;
 
     m_drive.drive(xSpeed, rot);
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
index 6e19020..e2fb7c8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -53,9 +53,9 @@
   /**
    * Places this motor controller in follower mode.
    *
-   * @param master The master to follow.
+   * @param leader The leader to follow.
    */
-  public void follow(ExampleSmartMotorController master) {
+  public void follow(ExampleSmartMotorController leader) {
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
index ac55100..3d52adb 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,16 +17,16 @@
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final ExampleSmartMotorController m_leftMaster =
+  private final ExampleSmartMotorController m_leftLeader =
       new ExampleSmartMotorController(DriveConstants.kLeftMotor1Port);
 
-  private final ExampleSmartMotorController m_leftSlave =
+  private final ExampleSmartMotorController m_leftFollower =
       new ExampleSmartMotorController(DriveConstants.kLeftMotor2Port);
 
-  private final ExampleSmartMotorController m_rightMaster =
+  private final ExampleSmartMotorController m_rightLeader =
       new ExampleSmartMotorController(DriveConstants.kRightMotor1Port);
 
-  private final ExampleSmartMotorController m_rightSlave =
+  private final ExampleSmartMotorController m_rightFollower =
       new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
 
   private final SimpleMotorFeedforward m_feedforward =
@@ -35,17 +35,17 @@
                                  DriveConstants.kaVoltSecondsSquaredPerMeter);
 
   // The robot's drive
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMaster, m_rightMaster);
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
 
   /**
    * Creates a new DriveSubsystem.
    */
   public DriveSubsystem() {
-    m_leftSlave.follow(m_leftMaster);
-    m_rightSlave.follow(m_rightMaster);
+    m_leftFollower.follow(m_leftLeader);
+    m_rightFollower.follow(m_rightLeader);
 
-    m_leftMaster.setPID(DriveConstants.kp, 0, 0);
-    m_rightMaster.setPID(DriveConstants.kp, 0, 0);
+    m_leftLeader.setPID(DriveConstants.kp, 0, 0);
+    m_rightLeader.setPID(DriveConstants.kp, 0, 0);
   }
 
   /**
@@ -65,10 +65,10 @@
    * @param right The right wheel state.
    */
   public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
-    m_leftMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+    m_leftLeader.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
                              left.position,
                              m_feedforward.calculate(left.velocity));
-    m_rightMaster.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
+    m_rightLeader.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
                               right.position,
                               m_feedforward.calculate(right.velocity));
   }
@@ -79,7 +79,7 @@
    * @return the left encoder distance
    */
   public double getLeftEncoderDistance() {
-    return m_leftMaster.getEncoderDistance();
+    return m_leftLeader.getEncoderDistance();
   }
 
   /**
@@ -88,15 +88,15 @@
    * @return the right encoder distance
    */
   public double getRightEncoderDistance() {
-    return m_rightMaster.getEncoderDistance();
+    return m_rightLeader.getEncoderDistance();
   }
 
   /**
    * Resets the drive encoders.
    */
   public void resetEncoders() {
-    m_leftMaster.resetEncoder();
-    m_rightMaster.resetEncoder();
+    m_leftLeader.resetEncoder();
+    m_rightLeader.resetEncoder();
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
new file mode 100644
index 0000000..22cde2e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
new file mode 100644
index 0000000..600437a
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.elevatorsimulation;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+/**
+ * This is a sample program to demonstrate the use of elevator simulation with existing code.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+  private static final int kJoystickPort = 0;
+
+  private static final double kElevatorKp = 5.0;
+  private static final double kElevatorGearing = 10.0;
+  private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
+  private static final double kCarriageMass = 4.0; // kg
+
+  private static final double kMinElevatorHeight = 0.0;
+  private static final double kMaxElevatorHeight = Units.inchesToMeters(50);
+
+  // distance per pulse = (distance per revolution) / (pulses per revolution)
+  //  = (Pi * D) / ppr
+  private static final double kElevatorEncoderDistPerPulse = 2.0 * Math.PI * kElevatorDrumRadius / 4096;
+
+  private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
+
+  // Standard classes for controlling our elevator
+  private final PIDController m_controller = new PIDController(kElevatorKp, 0, 0);
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+  private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  private final ElevatorSim m_elevatorSim = new ElevatorSim(m_elevatorGearbox,
+      kElevatorGearing,
+      kCarriageMass,
+      kElevatorDrumRadius,
+      kMinElevatorHeight,
+      kMaxElevatorHeight,
+      VecBuilder.fill(0.01));
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+  @Override
+  public void robotInit() {
+    m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse);
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_elevatorSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_elevatorSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    if (m_joystick.getTrigger()) {
+      // Here, we run PID control like normal, with a constant setpoint of 30in.
+      double pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.inchesToMeters(30));
+      m_motor.setVoltage(pidOutput);
+    } else {
+      // Otherwise, we disable the motor.
+      m_motor.set(0.0);
+    }
+  }
+
+  @Override
+  public void disabledInit() {
+    // This just makes sure that our simulation code knows that the motor's off.
+    m_motor.set(0.0);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
index d6a84ef..94e3c77 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -53,9 +53,9 @@
   /**
    * Places this motor controller in follower mode.
    *
-   * @param master The master to follow.
+   * @param leader The leader to follow.
    */
-  public void follow(ExampleSmartMotorController master) {
+  public void follow(ExampleSmartMotorController leader) {
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 6dd4178..5bc23ab 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -8,7 +8,7 @@
     "foldername": "gettingstarted",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Tank Drive",
@@ -22,7 +22,7 @@
     "foldername": "tankdrive",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Arcade Drive",
@@ -33,7 +33,7 @@
     "foldername": "arcadedrive",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Mecanum Drive",
@@ -47,7 +47,7 @@
     "foldername": "mecanumdrive",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "PDP CAN Monitoring",
@@ -60,7 +60,7 @@
     "foldername": "canpdp",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Solenoids",
@@ -74,7 +74,7 @@
     "foldername": "solenoid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Encoder",
@@ -87,7 +87,7 @@
     "foldername": "encoder",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Relay",
@@ -100,7 +100,7 @@
     "foldername": "relay",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Ultrasonic",
@@ -113,7 +113,7 @@
     "foldername": "ultrasonic",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Ultrasonic PID",
@@ -126,7 +126,7 @@
     "foldername": "ultrasonicpid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Potentiometer PID",
@@ -140,7 +140,7 @@
     "foldername": "potentiometerpid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with trapezoid profiled PID",
@@ -154,7 +154,7 @@
     "foldername": "elevatortrapezoidprofile",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Elevator with profiled PID controller",
@@ -168,7 +168,7 @@
     "foldername": "elevatorprofiledpid",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro",
@@ -182,7 +182,7 @@
     "foldername": "gyro",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Gyro Mecanum",
@@ -196,7 +196,7 @@
     "foldername": "gyromecanum",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "HID Rumble",
@@ -207,7 +207,7 @@
     "foldername": "hidrumble",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Motor Controller",
@@ -220,7 +220,7 @@
     "foldername": "motorcontrol",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Motor Control With Encoder",
@@ -236,7 +236,7 @@
     "foldername": "motorcontrolencoder",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "GearsBot",
@@ -270,7 +270,7 @@
     "foldername": "quickvision",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Intermediate Vision",
@@ -282,7 +282,7 @@
     "foldername": "intermediatevision",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Axis Camera Sample",
@@ -293,7 +293,7 @@
     "foldername": "axiscamera",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Shuffleboard Sample",
@@ -305,11 +305,11 @@
     "foldername": "shuffleboard",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "'Traditional' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'traditional' style, i.e. commands are given their own classes.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'traditional' style, i.e. commands are given their own classes.",
     "tags": [
       "Complete robot",
       "Command-based"
@@ -321,7 +321,7 @@
   },
   {
     "name": "'Inlined' Hatchbot",
-    "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "description": "A fully-functional command-based hatchbot for the 2019 game using the new command framework.  Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
     "tags": [
       "Complete robot",
       "Command-based",
@@ -334,7 +334,7 @@
   },
   {
     "name": "Select Command Example",
-    "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.",
+    "description": "An example showing how to use the SelectCommand class from the new command framework.",
     "tags": [
       "Command-based"
     ],
@@ -345,7 +345,7 @@
   },
   {
     "name": "Scheduler Event Logging",
-    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite",
+    "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the new command framework",
     "tags": [
       "Command-based",
       "Shuffleboard"
@@ -437,7 +437,7 @@
     "foldername": "arcadedrivexboxcontroller",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Tank Drive Xbox Controller",
@@ -448,7 +448,7 @@
     "foldername": "tankdrivexboxcontroller",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Duty Cycle Encoder",
@@ -551,5 +551,134 @@
     "gradlebase": "java",
     "mainclass": "Main",
     "commandversion": 2
+  },
+  {
+    "name": "RamseteController",
+    "description": "An example robot demonstrating the use of RamseteController.",
+    "tags": [
+      "RamseteController"
+    ],
+    "foldername": "ramsetecontroller",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceFlywheel",
+    "description": "An example state-space controller for a flywheel.",
+    "tags": [
+      "StateSpaceFlywheel",
+      "Flywheel",
+      "State Space",
+      "Model",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "statespaceflywheel",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceFlywheelSysId",
+    "description": "An example state-space controller for controlling a flywheel with System Identification.",
+    "tags": [
+      "StateSpaceFlywheelSysId",
+      "FRC Characterization",
+      "Flywheel",
+      "Characterization",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "statespaceflywheelsysid",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceElevator",
+    "description": "An example state-space controller for controlling an elevator.",
+    "tags": [
+      "Elevator",
+      "State Space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "statespaceelevator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceArm",
+    "description": "An example state-space controller for controlling an arm.",
+    "tags": [
+      "Arm",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick"
+    ],
+    "foldername": "statespacearm",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "StateSpaceDriveSimulation",
+    "description": "An example of drivetrain simulation in combination with a RAMSETE path following controller and the Field2d class.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick",
+      "Simulation"
+    ],
+    "foldername": "statespacedifferentialdrivesimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "ElevatorSimulation",
+    "description": "Demonstrates the use of physics simulation with a simple elevator.",
+    "tags": [
+      "Elevator",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Simulation",
+      "Physics"
+    ],
+    "foldername": "elevatorsimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "ArmSimulation",
+    "description": "Demonstrates the use of physics simulation with a simple single-jointedarm.",
+    "tags": [
+      "Arm",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Simulation",
+      "Physics"
+    ],
+    "foldername": "armsimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
index 69eb8ae..40d3f11 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -76,12 +76,6 @@
     SmartDashboard.putData(m_wrist);
     SmartDashboard.putData(m_claw);
 
-    // Call log method on all subsystems
-    m_wrist.log();
-    m_elevator.log();
-    m_drivetrain.log();
-    m_claw.log();
-
     // Configure the button bindings
     configureButtonBindings();
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
index b0d2515..86859f1 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -60,4 +60,12 @@
   public boolean isGrabbing() {
     return m_contact.get();
   }
+
+  /**
+   * Call log method every loop.
+   */
+  @Override
+  public void periodic() {
+    log();
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
index a0aaa32..9c85429 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -121,4 +121,12 @@
     // Really meters in simulation since it's a rangefinder...
     return m_rangefinder.getAverageVoltage();
   }
+
+  /**
+   * Call log method every loop.
+   */
+  @Override
+  public void periodic() {
+    log();
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
index 4f5c77f..5492046 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -75,4 +75,12 @@
   public void useOutput(double output, double setpoint) {
     m_motor.set(output);
   }
+
+  /**
+   * Call log method every loop.
+   */
+  @Override
+  public void periodic() {
+    log();
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
index dbd952e..1b32139 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -72,4 +72,12 @@
   public void useOutput(double output, double setpoint) {
     m_motor.set(output);
   }
+
+  /**
+   * Call log method every loop.
+   */
+  @Override
+  public void periodic() {
+    log();
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
index 079318a..622fda4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,7 +15,7 @@
 
 /**
  * This is a sample program that uses mecanum drive with a gyro sensor to
- * maintian rotation vectorsin relation to the starting orientation of the robot
+ * maintain rotation vectorsin relation to the starting orientation of the robot
  * (field-oriented controls).
  */
 public class Robot extends TimedRobot {

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
index 2250123..d49fba4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -78,7 +78,7 @@
                          m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
 
     // Add commands to the autonomous command chooser
-    m_chooser.addOption("Simple Auto", m_simpleAuto);
+    m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
     m_chooser.addOption("Complex Auto", m_complexAuto);
 
     // Put the chooser on the dashboard

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
index 3e2dc82..0784dde 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -72,7 +72,7 @@
             () -> m_driverController.getX(GenericHID.Hand.kRight)));
 
     // Add commands to the autonomous command chooser
-    m_chooser.addOption("Simple Auto", m_simpleAuto);
+    m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
     m_chooser.addOption("Complex Auto", m_complexAuto);
 
     // Put the chooser on the dashboard

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
index 6220807..2a081ff 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
@@ -54,7 +54,10 @@
   );
 
   private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
-      getAngle());
+      m_gyro.getRotation2d());
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
 
   /**
    * Constructs a MecanumDrive and resets the gyro.
@@ -64,16 +67,6 @@
   }
 
   /**
-   * Returns the angle of the robot as a Rotation2d.
-   *
-   * @return The angle of the robot.
-   */
-  public Rotation2d getAngle() {
-    // Negating the angle because WPILib gyros are CW positive.
-    return Rotation2d.fromDegrees(-m_gyro.getAngle());
-  }
-
-  /**
    * Returns the current state of the drivetrain.
    *
    * @return The current state of the drivetrain.
@@ -93,23 +86,28 @@
    * @param speeds The desired wheel speeds.
    */
   public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
-    final var frontLeftOutput = m_frontLeftPIDController.calculate(
+    final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
+    final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
+    final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
+    final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
+
+    final double frontLeftOutput = m_frontLeftPIDController.calculate(
         m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond
     );
-    final var frontRightOutput = m_frontRightPIDController.calculate(
+    final double frontRightOutput = m_frontRightPIDController.calculate(
         m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond
     );
-    final var backLeftOutput = m_backLeftPIDController.calculate(
+    final double backLeftOutput = m_backLeftPIDController.calculate(
         m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond
     );
-    final var backRightOutput = m_backRightPIDController.calculate(
+    final double backRightOutput = m_backRightPIDController.calculate(
         m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond
     );
 
-    m_frontLeftMotor.set(frontLeftOutput);
-    m_frontRightMotor.set(frontRightOutput);
-    m_backLeftMotor.set(backLeftOutput);
-    m_backRightMotor.set(backRightOutput);
+    m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
+    m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
+    m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
+    m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
   }
 
   /**
@@ -124,7 +122,7 @@
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
     var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(
         fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            xSpeed, ySpeed, rot, getAngle()
+            xSpeed, ySpeed, rot, m_gyro.getRotation2d()
         ) : new ChassisSpeeds(xSpeed, ySpeed, rot)
     );
     mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
@@ -135,6 +133,6 @@
    * Updates the field relative position of the robot.
    */
   public void updateOdometry() {
-    m_odometry.update(getAngle(), getCurrentState());
+    m_odometry.update(m_gyro.getRotation2d(), getCurrentState());
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
index aeb4681..d41a947 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.examples.mecanumbot;
 
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
@@ -15,6 +16,11 @@
   private final XboxController m_controller = new XboxController(0);
   private final Drivetrain m_mecanum = new Drivetrain();
 
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
   @Override
   public void autonomousPeriodic() {
     driveWithJoystick(false);
@@ -29,18 +35,24 @@
   private void driveWithJoystick(boolean fieldRelative) {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
+    final var xSpeed =
+        -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
+            * Drivetrain.kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
-    final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
+    final var ySpeed =
+        -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
+            * Drivetrain.kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed;
+    final var rot =
+        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
+            * Drivetrain.kMaxAngularSpeed;
 
     m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
index 88f904b..b402109 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -55,8 +55,6 @@
         // Assumes the encoders are directly mounted on the wheel shafts
         (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
 
-    public static final boolean kGyroReversed = false;
-
     // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
     // These characterization values MUST be determined either experimentally or theoretically
     // for *your* robot's drive.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
index 1ec1fc9..91055c6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -133,6 +133,9 @@
         m_robotDrive
     );
 
+    // Reset odometry to the starting pose of the trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
index 7aaf114..1fac67e 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,6 @@
 import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.drive.MecanumDrive;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
 import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
@@ -62,7 +61,7 @@
 
   // Odometry class for tracking robot pose
   MecanumDriveOdometry m_odometry =
-      new MecanumDriveOdometry(DriveConstants.kDriveKinematics, getAngle());
+      new MecanumDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
 
   /**
    * Creates a new DriveSubsystem.
@@ -75,20 +74,10 @@
     m_rearRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
   }
 
-  /**
-   * Returns the angle of the robot as a Rotation2d.
-   *
-   * @return The angle of the robot.
-   */
-  public Rotation2d getAngle() {
-    // Negating the angle because WPILib gyros are CW positive.
-    return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? 1.0 : -1.0));
-  }
-
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
-    m_odometry.update(getAngle(),
+    m_odometry.update(m_gyro.getRotation2d(),
                       new MecanumDriveWheelSpeeds(
                           m_frontLeftEncoder.getRate(),
                           m_rearLeftEncoder.getRate(),
@@ -111,7 +100,7 @@
    * @param pose The pose to which to set the odometry.
    */
   public void resetOdometry(Pose2d pose) {
-    m_odometry.resetPosition(pose, getAngle());
+    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
   }
 
   /**
@@ -227,10 +216,10 @@
   /**
    * Returns the heading of the robot.
    *
-   * @return the robot's heading in degrees, from 180 to 180
+   * @return the robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
-    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    return m_gyro.getRotation2d().getDegrees();
   }
 
   /**
@@ -239,6 +228,6 @@
    * @return The turn rate of the robot, in degrees per second
    */
   public double getTurnRate() {
-    return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    return -m_gyro.getRate();
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
index 5c70e71..6fa7e68 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -22,7 +22,6 @@
  * <p>NOTE: Simulation currently approximates this as as single pneumatic
  * cylinder and ignores the latch.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class Shooter extends Subsystem {
   // Devices
   DoubleSolenoid m_piston1 = new DoubleSolenoid(1, 3, 4);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
index f70a003..49044a2 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -39,8 +39,6 @@
         // Assumes the encoders are directly mounted on the wheel shafts
         (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
 
-    public static final boolean kGyroReversed = true;
-
     // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
     // These characterization values MUST be determined either experimentally or theoretically
     // for *your* robot's drive.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index 8bb4e1d..bb8dd28 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -135,6 +135,9 @@
         m_robotDrive
     );
 
+    // Reset odometry to the starting pose of the trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index 7b5c5e5..396d5df 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,7 +13,6 @@
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
@@ -60,13 +59,13 @@
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 
     resetEncoders();
-    m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
+    m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
   }
 
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
-    m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
+    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
                       m_rightEncoder.getDistance());
   }
 
@@ -95,7 +94,7 @@
    */
   public void resetOdometry(Pose2d pose) {
     resetEncoders();
-    m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
+    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
   }
 
   /**
@@ -117,6 +116,7 @@
   public void tankDriveVolts(double leftVolts, double rightVolts) {
     m_leftMotors.setVoltage(leftVolts);
     m_rightMotors.setVoltage(-rightVolts);
+    m_drive.feed();
   }
 
   /**
@@ -173,10 +173,10 @@
   /**
    * Returns the heading of the robot.
    *
-   * @return the robot's heading in degrees, from 180 to 180
+   * @return the robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
-    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    return m_gyro.getRotation2d().getDegrees();
   }
 
   /**
@@ -185,6 +185,6 @@
    * @return The turn rate of the robot, in degrees per second
    */
   public double getTurnRate() {
-    return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    return -m_gyro.getRate();
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
new file mode 100644
index 0000000..388b32d
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecontroller;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // meters per second
+  public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+  private static final double kTrackWidth = 0.381 * 2; // meters
+  private static final double kWheelRadius = 0.0508; // meters
+  private static final int kEncoderResolution = 4096;
+
+  private final SpeedController m_leftLeader = new PWMVictorSPX(1);
+  private final SpeedController m_leftFollower = new PWMVictorSPX(2);
+  private final SpeedController m_rightLeader = new PWMVictorSPX(3);
+  private final SpeedController m_rightFollower = new PWMVictorSPX(4);
+
+  private final Encoder m_leftEncoder = new Encoder(0, 1);
+  private final Encoder m_rightEncoder = new Encoder(2, 3);
+
+  private final SpeedControllerGroup m_leftGroup
+      = new SpeedControllerGroup(m_leftLeader, m_leftFollower);
+  private final SpeedControllerGroup m_rightGroup
+      = new SpeedControllerGroup(m_rightLeader, m_rightFollower);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+  private final DifferentialDriveKinematics m_kinematics
+      = new DifferentialDriveKinematics(kTrackWidth);
+
+  private final DifferentialDriveOdometry m_odometry;
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  /**
+   * Constructs a differential drive object.
+   * Sets the encoder distance per pulse and resets the gyro.
+   */
+  public Drivetrain() {
+    m_gyro.reset();
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+    m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+
+    m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
+  }
+
+  /**
+   * Sets the desired wheel speeds.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+    final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+    final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+
+    final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
+        speeds.leftMetersPerSecond);
+    final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
+        speeds.rightMetersPerSecond);
+    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
+    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+  }
+
+  /**
+   * Drives the robot with the given linear velocity and angular velocity.
+   *
+   * @param xSpeed Linear velocity in m/s.
+   * @param rot    Angular velocity in rad/s.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double rot) {
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+    setSpeeds(wheelSpeeds);
+  }
+
+  /**
+   * Updates the field-relative position.
+   */
+  public void updateOdometry() {
+    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
+        m_rightEncoder.getDistance());
+  }
+
+  /**
+   * Resets the field-relative position to a specific location.
+   * @param pose The position to reset to.
+   */
+  public void resetOdometry(Pose2d pose) {
+    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
+  }
+
+  /**
+   * Returns the pose of the robot.
+   * @return The pose of the robot.
+   */
+  public Pose2d getPose() {
+    return m_odometry.getPoseMeters();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
new file mode 100644
index 0000000..bd07c2e
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecontroller;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
new file mode 100644
index 0000000..6a95a24
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecontroller;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.util.Units;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_drive = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  // An example trajectory to follow during the autonomous period.
+  private Trajectory m_trajectory;
+
+  // The Ramsete Controller to follow the trajectory.
+  private RamseteController m_ramseteController;
+
+  // The timer to use during the autonomous period.
+  private Timer m_timer;
+
+  @Override
+  public void robotInit() {
+    // Create the trajectory to follow in autonomous. It is best to initialize
+    // trajectories here to avoid wasting time in autonomous.
+    m_trajectory = TrajectoryGenerator.generateTrajectory(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+        List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+        new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
+        new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))
+    );
+  }
+
+  @Override
+  public void autonomousInit() {
+    // Initialize the timer.
+    m_timer = new Timer();
+    m_timer.start();
+
+    // Reset the drivetrain's odometry to the starting pose of the trajectory.
+    m_drive.resetOdometry(m_trajectory.getInitialPose());
+  }
+
+  @Override
+  public void autonomousPeriodic() {
+    // Update odometry.
+    m_drive.updateOdometry();
+
+    if (m_timer.get() < m_trajectory.getTotalTimeSeconds()) {
+      // Get the desired pose from the trajectory.
+      var desiredPose = m_trajectory.sample(m_timer.get());
+
+      // Get the reference chassis speeds from the Ramsete controller.
+      var refChassisSpeeds = m_ramseteController.calculate(m_drive.getPose(), desiredPose);
+
+      // Set the linear and angular speeds.
+      m_drive.drive(refChassisSpeeds.vxMetersPerSecond, refChassisSpeeds.omegaRadiansPerSecond);
+    } else {
+      m_drive.drive(0, 0);
+    }
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed =
+        -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
+            * Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot =
+        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
+            * Drivetrain.kMaxAngularSpeed;
+
+    m_drive.drive(xSpeed, rot);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
index 37142e8..eb26912 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -15,8 +15,6 @@
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
 
-import static java.util.Map.entry;
-
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -43,9 +41,9 @@
       new SelectCommand(
           // Maps selector values to commands
           Map.ofEntries(
-              entry(CommandSelector.ONE, new PrintCommand("Command one was selected!")),
-              entry(CommandSelector.TWO, new PrintCommand("Command two was selected!")),
-              entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))
+              Map.entry(CommandSelector.ONE, new PrintCommand("Command one was selected!")),
+              Map.entry(CommandSelector.TWO, new PrintCommand("Command two was selected!")),
+              Map.entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))
           ),
           this::select
       );

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
new file mode 100644
index 0000000..c8a6fb1
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespacearm;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
new file mode 100644
index 0000000..508102d
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespacearm;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
+import edu.wpi.first.wpilibj.estimator.KalmanFilter;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.LinearSystemLoop;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control an arm.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+  private static final int kJoystickPort = 0;
+  private static final double kRaisedPosition = Units.degreesToRadians(90.0);
+  private static final double kLoweredPosition = Units.degreesToRadians(0.0);
+
+  // Moment of inertia of the arm, in kg * m^2. Can be estimated with CAD. If finding this constant
+  // is difficult, LinearSystem.identifyPositionSystem may be better.
+  private static final double kArmMOI = 1.2;
+
+  // Reduction between motors and encoder, as output over input. If the arm spins slower than
+  // the motors, this number should be greater than one.
+  private static final double kArmGearing = 10.0;
+
+  private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
+        Units.degreesToRadians(45), Units.degreesToRadians(90)); // Max arm speed and acceleration.
+  private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
+
+  // The plant holds a state-space model of our arm. This system has the following properties:
+  //
+  // States: [position, velocity], in radians and radians per second.
+  // Inputs (what we can "put in"): [voltage], in volts.
+  // Outputs (what we can measure): [position], in radians.
+  private final LinearSystem<N2, N1, N1> m_armPlant =
+      LinearSystemId.createSingleJointedArmSystem(
+        DCMotor.getNEO(2),
+        kArmMOI,
+        kArmGearing);
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
+        Nat.N2(), Nat.N1(),
+        m_armPlant,
+        VecBuilder.fill(0.015, 0.17), // How accurate we
+        // think our model is, in radians and radians/sec
+        VecBuilder.fill(0.01), // How accurate we think our encoder position
+        // data is. In this case we very highly trust our encoder position reading.
+        0.020);
+
+  // A LQR uses feedback to create voltage commands.
+  private final LinearQuadraticRegulator<N2, N1, N1> m_controller
+        = new LinearQuadraticRegulator<>(m_armPlant,
+        VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
+        // Position and velocity error tolerances, in radians and radians per second. Decrease this
+        // to more heavily penalize state excursion, or make the controller behave more
+        // aggressively. In this example we weight position much more highly than velocity, but this
+        // can be tuned to balance the two.
+        VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+        // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+        // starting point because that is the (approximate) maximum voltage of a battery.
+        0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+  // lower if using notifiers.
+
+  // The state-space loop combines a controller, observer, feedforward and plant for easy control.
+  private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
+      m_armPlant,
+        m_controller,
+        m_observer,
+        12.0,
+        0.020);
+
+  // An encoder set up to measure arm position in radians.
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+
+  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+
+  // A joystick to read the trigger from.
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  @Override
+  public void robotInit() {
+    // We go 2 pi radians in 1 rotation, or 4096 counts.
+    m_encoder.setDistancePerPulse(Math.PI * 2 / 4096.0);
+  }
+
+  @Override
+  public void teleopInit() {
+    // Reset our loop to make sure it's in a known state.
+    m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
+
+    // Reset our last reference to the current state.
+    m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
+          m_encoder.getRate());
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Sets the target position of our arm. This is similar to setting the setpoint of a
+    // PID controller.
+    TrapezoidProfile.State goal;
+    if (m_joystick.getTrigger()) {
+      // the trigger is pressed, so we go to the high goal.
+      goal = new TrapezoidProfile.State(kRaisedPosition, 0.0);
+    } else {
+      // Otherwise, we go to the low goal
+      goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
+    }
+    // Step our TrapezoidalProfile forward 20ms and set it as our next reference
+    m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
+          .calculate(0.020);
+    m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to predict the next
+    // state with out Kalman filter.
+    m_loop.predict(0.020);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    double nextVoltage = m_loop.getU(0);
+    m_motor.setVoltage(nextVoltage);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
new file mode 100644
index 0000000..b86bd3f
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final double kTrackwidthMeters = 0.69;
+    public static final DifferentialDriveKinematics kDriveKinematics =
+          new DifferentialDriveKinematics(kTrackwidthMeters);
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterMeters = 0.15;
+    public static final double kEncoderDistancePerPulse =
+          // Assumes the encoders are directly mounted on the wheel shafts
+          (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+
+    public static final boolean kGyroReversed = true;
+
+    // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+    // These characterization values MUST be determined either experimentally or theoretically
+    // for *your* robot's drive.
+    // The Robot Characterization Toolsuite provides a convenient tool for obtaining these
+    // values for your robot.
+    public static final double ksVolts = 0.22;
+    public static final double kvVoltSecondsPerMeter = 1.98;
+    public static final double kaVoltSecondsSquaredPerMeter = 0.2;
+
+    // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+    // These characterization values MUST be determined either experimentally or theoretically
+    // for *your* robot's drive.
+    // These two values are "angular" kV and kA
+    public static final double kvVoltSecondsPerRadian = 1.5;
+    public static final double kaVoltSecondsSquaredPerRadian = 0.3;
+
+    public static final LinearSystem<N2, N2, N2> kDrivetrainPlant =
+          LinearSystemId.identifyDrivetrainSystem(kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter,
+                kvVoltSecondsPerRadian, kaVoltSecondsSquaredPerRadian);
+
+    // Example values only -- use what's on your physical robot!
+    public static final DCMotor kDriveGearbox = DCMotor.getCIM(2);
+    public static final double kDriveGearing = 8;
+
+    // Example value only - as above, this must be tuned for your drive!
+    public static final double kPDriveVel = 0.1;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+
+  public static final class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 6;
+
+    // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
+    public static final double kRamseteB = 2;
+    public static final double kRamseteZeta = 0.7;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
new file mode 100644
index 0000000..5b041fb
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
new file mode 100644
index 0000000..485dfc3
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * This is a sample program to demonstrate the use of state-space classes in robot simulation.
+ * This robot has a flywheel, elevator, arm and differential drivetrain, and interfaces with
+ * the sim GUI's {@link edu.wpi.first.wpilibj.simulation.Field2d} class.
+ */
+public class Robot extends TimedRobot {
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // Here we calculate the battery voltage based on drawn current.
+    // As our robot draws more power from the battery its voltage drops.
+    // The estimated voltage is highly dependent on the battery's internal
+    // resistance.
+    double drawCurrent = m_robotContainer.getRobotDrive().getDrawnCurrentAmps();
+    double loadedVoltage = BatterySim.calculateDefaultBatteryLoadedVoltage(drawCurrent);
+    RoboRioSim.setVInVoltage(loadedVoltage);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    CommandScheduler.getInstance().run();
+  }
+
+  @Override
+  public void autonomousInit() {
+    m_robotContainer.getAutonomousCommand().schedule();
+  }
+
+  @Override
+  public void disabledInit() {
+    CommandScheduler.getInstance().cancelAll();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
new file mode 100644
index 0000000..0790a40
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
@@ -0,0 +1,139 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import java.util.List;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the Robot
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+  // The driver's controller
+  XboxController m_driverController = new XboxController(Constants.OIConstants.kDriverControllerPort);
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        new RunCommand(() -> m_robotDrive
+            .arcadeDrive(-m_driverController.getY(GenericHID.Hand.kRight),
+                m_driverController.getX(GenericHID.Hand.kLeft)), m_robotDrive));
+
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * {@link JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Drive at half speed when the right bumper is held
+    new JoystickButton(m_driverController, Button.kBumperRight.value)
+        .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
+        .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+
+  }
+
+  public DriveSubsystem getRobotDrive() {
+    return m_robotDrive;
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+
+    // Create a voltage constraint to ensure we don't accelerate too fast
+    var autoVoltageConstraint =
+        new DifferentialDriveVoltageConstraint(
+            new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts,
+                Constants.DriveConstants.kvVoltSecondsPerMeter,
+                Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
+            Constants.DriveConstants.kDriveKinematics,
+            7);
+
+    // Create config for trajectory
+    TrajectoryConfig config =
+        new TrajectoryConfig(Constants.AutoConstants.kMaxSpeedMetersPerSecond,
+            Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+            // Add kinematics to ensure max speed is actually obeyed
+            .setKinematics(Constants.DriveConstants.kDriveKinematics)
+            // Apply the voltage constraint
+            .addConstraint(autoVoltageConstraint);
+
+    // An example trajectory to follow.  All units in meters.
+    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+        // Start at the origin facing the +X direction
+        new Pose2d(0, 0, new Rotation2d(0)),
+        // Pass through these two interior waypoints, making an 's' curve path
+        List.of(),
+        // End 3 meters straight ahead of where we started, facing forward
+        new Pose2d(6, 6, new Rotation2d(0)),
+        // Pass config
+        config
+    );
+
+    RamseteCommand ramseteCommand = new RamseteCommand(
+        exampleTrajectory,
+        m_robotDrive::getPose,
+        new RamseteController(Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta),
+        new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts,
+            Constants.DriveConstants.kvVoltSecondsPerMeter,
+            Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
+        Constants.DriveConstants.kDriveKinematics,
+        m_robotDrive::getWheelSpeeds,
+        new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
+        new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
+        // RamseteCommand passes volts to the callback
+        m_robotDrive::tankDriveVolts,
+        m_robotDrive
+    );
+
+    // Reset odometry to starting pose of trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
+    // Run path following command, then stop at the end.
+    return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000..aaa0d8c
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
@@ -0,0 +1,258 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.Field2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final SpeedControllerGroup m_leftMotors =
+        new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kLeftMotor1Port),
+              new PWMVictorSPX(Constants.DriveConstants.kLeftMotor2Port));
+
+  // The motors on the right side of the drive.
+  private final SpeedControllerGroup m_rightMotors =
+        new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kRightMotor1Port),
+              new PWMVictorSPX(Constants.DriveConstants.kRightMotor2Port));
+
+  // The robot's drive
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+        new Encoder(Constants.DriveConstants.kLeftEncoderPorts[0],
+              Constants.DriveConstants.kLeftEncoderPorts[1],
+              Constants.DriveConstants.kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+        new Encoder(Constants.DriveConstants.kRightEncoderPorts[0],
+              Constants.DriveConstants.kRightEncoderPorts[1],
+              Constants.DriveConstants.kRightEncoderReversed);
+
+  // The gyro sensor
+  private final Gyro m_gyro = new ADXRS450_Gyro();
+
+  // Odometry class for tracking robot pose
+  private final DifferentialDriveOdometry m_odometry;
+
+  // These classes help us simulate our drivetrain
+  public DifferentialDrivetrainSim m_drivetrainSimulator;
+  private EncoderSim m_leftEncoderSim;
+  private EncoderSim m_rightEncoderSim;
+  // The Field2d class simulates the field in the sim GUI. Note that we can have only one
+  // instance!
+  private Field2d m_fieldSim;
+  private SimDouble m_gyroAngleSim;
+
+  /**
+   * Creates a new DriveSubsystem.
+   */
+  public DriveSubsystem() {
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
+
+    resetEncoders();
+    m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
+
+    if (RobotBase.isSimulation()) { // If our robot is simulated
+      // This class simulates our drivetrain's motion around the field.
+      m_drivetrainSimulator = new DifferentialDrivetrainSim(
+            Constants.DriveConstants.kDrivetrainPlant,
+            Constants.DriveConstants.kDriveGearbox,
+            Constants.DriveConstants.kDriveGearing,
+            Constants.DriveConstants.kTrackwidthMeters,
+          Constants.DriveConstants.kWheelDiameterMeters / 2.0);
+
+      // The encoder and gyro angle sims let us set simulated sensor readings
+      m_leftEncoderSim = new EncoderSim(m_leftEncoder);
+      m_rightEncoderSim = new EncoderSim(m_rightEncoder);
+      m_gyroAngleSim =
+            new SimDeviceSim("ADXRS450_Gyro" + "[" + SPI.Port.kOnboardCS0.value + "]")
+                  .getDouble("Angle");
+
+      // the Field2d class lets us visualize our robot in the simulation GUI.
+      m_fieldSim = new Field2d();
+    }
+  }
+
+  @Override
+  public void periodic() {
+    // Update the odometry in the periodic block
+    m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
+          m_rightEncoder.getDistance());
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // To update our simulation, we set motor voltage inputs, update the simulation,
+    // and write the simulated positions and velocities to our simulated encoder and gyro.
+    // We negate the right side so that positive voltages make the right side
+    // move forward.
+    m_drivetrainSimulator.setInputs(m_leftMotors.get() * RobotController.getBatteryVoltage(),
+          -m_rightMotors.get() * RobotController.getBatteryVoltage());
+    m_drivetrainSimulator.update(0.020);
+
+    m_leftEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftPosition));
+    m_leftEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftVelocity));
+    m_rightEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightPosition));
+    m_rightEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightVelocity));
+    m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees());
+
+    m_fieldSim.setRobotPose(getPose());
+  }
+
+  /**
+   * Returns the current being drawn by the drivetrain. This works in SIMULATION ONLY!
+   * If you want it to work elsewhere, use the code in
+   * {@link DifferentialDrivetrainSim#getCurrentDrawAmps()}
+   *
+   * @return The drawn current in Amps.
+   */
+  public double getDrawnCurrentAmps() {
+    return m_drivetrainSimulator.getCurrentDrawAmps();
+  }
+
+  /**
+   * Returns the currently-estimated pose of the robot.
+   *
+   * @return The pose.
+   */
+  public Pose2d getPose() {
+    return m_odometry.getPoseMeters();
+  }
+
+  /**
+   * Returns the current wheel speeds of the robot.
+   *
+   * @return The current wheel speeds.
+   */
+  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
+    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
+  }
+
+  /**
+   * Resets the odometry to the specified pose.
+   *
+   * @param pose The pose to which to set the odometry.
+   */
+  public void resetOdometry(Pose2d pose) {
+    resetEncoders();
+    m_drivetrainSimulator.setPose(pose);
+    m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /**
+   * Controls the left and right sides of the drive directly with voltages.
+   *
+   * @param leftVolts  the commanded left output
+   * @param rightVolts the commanded right output
+   */
+  public void tankDriveVolts(double leftVolts, double rightVolts) {
+    var batteryVoltage = RobotController.getBatteryVoltage();
+    if (Math.max(Math.abs(leftVolts), Math.abs(rightVolts))
+          > batteryVoltage) {
+      leftVolts *= batteryVoltage / 12.0;
+      rightVolts *= batteryVoltage / 12.0;
+    }
+    m_leftMotors.setVoltage(leftVolts);
+    m_rightMotors.setVoltage(-rightVolts);
+    m_drive.feed();
+  }
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the two encoders.
+   *
+   * @return the average of the two encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0;
+  }
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  public Encoder getLeftEncoder() {
+    return m_leftEncoder;
+  }
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  public Encoder getRightEncoder() {
+    return m_rightEncoder;
+  }
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+
+  /**
+   * Zeroes the heading of the robot.
+   */
+  public void zeroHeading() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Returns the heading of the robot.
+   *
+   * @return the robot's heading in degrees, from -180 to 180
+   */
+  public double getHeading() {
+    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (Constants.DriveConstants.kGyroReversed ? -1.0 : 1.0);
+  }
+
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
new file mode 100644
index 0000000..03dff05
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespaceelevator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
new file mode 100644
index 0000000..85cbff4
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespaceelevator;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
+import edu.wpi.first.wpilibj.estimator.KalmanFilter;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.LinearSystemLoop;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control an elevator.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+  private static final int kJoystickPort = 0;
+  private static final double kHighGoalPosition = Units.feetToMeters(3);
+  private static final double kLowGoalPosition = Units.feetToMeters(0);
+
+  private static final double kCarriageMass = 4.5; // kilograms
+
+  // A 1.5in diameter drum has a radius of 0.75in, or 0.019in.
+  private static final double kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0;
+
+  // Reduction between motors and encoder, as output over input. If the elevator spins slower than
+  // the motors, this number should be greater than one.
+  private static final double kElevatorGearing = 6.0;
+
+  private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
+        Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
+  private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
+
+  /* The plant holds a state-space model of our elevator. This system has the following properties:
+
+  States: [position, velocity], in meters and meters per second.
+  Inputs (what we can "put in"): [voltage], in volts.
+  Outputs (what we can measure): [position], in meters.
+
+  This elevator is driven by two NEO motors.
+   */
+  private final LinearSystem<N2, N1, N1> m_elevatorPlant = LinearSystemId.createElevatorSystem(
+        DCMotor.getNEO(2),
+        kCarriageMass,
+        kDrumRadius,
+        kElevatorGearing
+  );
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
+        Nat.N2(), Nat.N1(),
+        m_elevatorPlant,
+        VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
+        // think our model is, in meters and meters/second.
+        VecBuilder.fill(0.001), // How accurate we think our encoder position
+        // data is. In this case we very highly trust our encoder position reading.
+        0.020);
+
+  // A LQR uses feedback to create voltage commands.
+  private final LinearQuadraticRegulator<N2, N1, N1> m_controller
+        = new LinearQuadraticRegulator<>(m_elevatorPlant,
+        VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
+        // and velocity error tolerances, in meters and meters per second. Decrease this to more
+        // heavily penalize state excursion, or make the controller behave more aggressively. In
+        // this example we weight position much more highly than velocity, but this can be
+        // tuned to balance the two.
+        VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+        // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+        // starting point because that is the (approximate) maximum voltage of a battery.
+        0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+  // lower if using notifiers.
+
+  // The state-space loop combines a controller, observer, feedforward and plant for easy control.
+  private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
+      m_elevatorPlant,
+        m_controller,
+        m_observer,
+        12.0,
+        0.020);
+
+  // An encoder set up to measure elevator height in meters.
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+
+  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+
+  // A joystick to read the trigger from.
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  @Override
+  public void robotInit() {
+    // Circumference = pi * d, so distance per click = pi * d / counts
+    m_encoder.setDistancePerPulse(Math.PI * 2 * kDrumRadius / 4096.0);
+  }
+
+  @Override
+  public void teleopInit() {
+    // Reset our loop to make sure it's in a known state.
+    m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
+
+    // Reset our last reference to the current state.
+    m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
+          m_encoder.getRate());
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Sets the target position of our arm. This is similar to setting the setpoint of a
+    // PID controller.
+    TrapezoidProfile.State goal;
+    if (m_joystick.getTrigger()) {
+      // the trigger is pressed, so we go to the high goal.
+      goal = new TrapezoidProfile.State(kHighGoalPosition, 0.0);
+    } else {
+      // Otherwise, we go to the low goal
+      goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
+    }
+    // Step our TrapezoidalProfile forward 20ms and set it as our next reference
+    m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
+          .calculate(0.020);
+    m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to predict the next
+    // state with out Kalman filter.
+    m_loop.predict(0.020);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    double nextVoltage = m_loop.getU(0);
+    m_motor.setVoltage(nextVoltage);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
new file mode 100644
index 0000000..3673388
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespaceflywheel;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
new file mode 100644
index 0000000..f9d1131
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespaceflywheel;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
+import edu.wpi.first.wpilibj.estimator.KalmanFilter;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.LinearSystemLoop;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control a flywheel.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+  private static final int kJoystickPort = 0;
+  private static final double kSpinupRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(500.0);
+
+  private static final double kFlywheelMomentOfInertia = 0.00032; // kg * m^2
+
+  // Reduction between motors and encoder, as output over input. If the flywheel spins slower than
+  // the motors, this number should be greater than one.
+  private static final double kFlywheelGearing = 1.0;
+
+  // The plant holds a state-space model of our flywheel. This system has the following properties:
+  //
+  // States: [velocity], in radians per second.
+  // Inputs (what we can "put in"): [voltage], in volts.
+  // Outputs (what we can measure): [velocity], in radians per second.
+  private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.createFlywheelSystem(
+        DCMotor.getNEO(2),
+        kFlywheelMomentOfInertia,
+        kFlywheelGearing);
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
+        Nat.N1(), Nat.N1(),
+        m_flywheelPlant,
+        VecBuilder.fill(3.0), // How accurate we think our model is
+        VecBuilder.fill(0.01), // How accurate we think our encoder
+        // data is
+        0.020);
+
+  // A LQR uses feedback to create voltage commands.
+  private final LinearQuadraticRegulator<N1, N1, N1> m_controller
+        = new LinearQuadraticRegulator<>(m_flywheelPlant,
+        VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
+        // this to more heavily penalize state excursion, or make the controller behave more
+        // aggressively.
+        VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+        // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+        // starting point because that is the (approximate) maximum voltage of a battery.
+        0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+  // lower if using notifiers.
+
+  // The state-space loop combines a controller, observer, feedforward and plant for easy control.
+  private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
+      m_flywheelPlant,
+        m_controller,
+        m_observer,
+        12.0,
+        0.020);
+
+  // An encoder set up to measure flywheel velocity in radians per second.
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+
+  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+
+  // A joystick to read the trigger from.
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  @Override
+  public void robotInit() {
+    // We go 2 pi radians per 4096 clicks.
+    m_encoder.setDistancePerPulse(
+          2.0 * Math.PI / 4096.0);
+  }
+
+  @Override
+  public void teleopInit() {
+    m_loop.reset(VecBuilder.fill(m_encoder.getRate()));
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Sets the target speed of our flywheel. This is similar to setting the setpoint of a
+    // PID controller.
+    if (m_joystick.getTriggerPressed()) {
+      // We just pressed the trigger, so let's set our next reference
+      m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
+    } else if (m_joystick.getTriggerReleased()) {
+      // We just released the trigger, so let's spin down
+      m_loop.setNextR(VecBuilder.fill(0.0));
+    }
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to predict the next
+    // state with out Kalman filter.
+    m_loop.predict(0.020);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    double nextVoltage = m_loop.getU(0);
+    m_motor.setVoltage(nextVoltage);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
new file mode 100644
index 0000000..e487e9f
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
new file mode 100644
index 0000000..7cd773b
--- /dev/null
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
+import edu.wpi.first.wpilibj.estimator.KalmanFilter;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.LinearSystemLoop;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * This is a sample program to demonstrate how to use a state-space controller
+ * to control a flywheel.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+  private static final int kJoystickPort = 0;
+  private static final double kSpinupRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(500.0);
+
+  // Volts per (radian per second)
+  private static final double kFlywheelKv = 0.023;
+
+  // Volts per (radian per second squared)
+  private static final double kFlywheelKa = 0.001;
+
+  // The plant holds a state-space model of our flywheel. This system has the following properties:
+  //
+  // States: [velocity], in radians per second.
+  // Inputs (what we can "put in"): [voltage], in volts.
+  // Outputs (what we can measure): [velocity], in radians per second.
+  //
+  // The Kv and Ka constants are found using the FRC Characterization toolsuite.
+  private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.identifyVelocitySystem(
+        kFlywheelKv, kFlywheelKa);
+
+  // The observer fuses our encoder data and voltage inputs to reject noise.
+  private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
+        Nat.N1(), Nat.N1(),
+        m_flywheelPlant,
+        VecBuilder.fill(3.0), // How accurate we think our model is
+        VecBuilder.fill(0.01), // How accurate we think our encoder
+        // data is
+        0.020);
+
+  // A LQR uses feedback to create voltage commands.
+  private final LinearQuadraticRegulator<N1, N1, N1> m_controller
+        = new LinearQuadraticRegulator<>(m_flywheelPlant,
+        VecBuilder.fill(8.0), // Velocity error tolerance
+        VecBuilder.fill(12.0), // Control effort (voltage) tolerance
+        0.020);
+
+  // The state-space loop combines a controller, observer, feedforward and plant for easy control.
+  private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
+      m_flywheelPlant,
+        m_controller,
+        m_observer,
+        12.0,
+        0.020);
+
+  // An encoder set up to measure flywheel velocity in radians per second.
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+
+  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+
+  // A joystick to read the trigger from.
+  private final Joystick m_joystick = new Joystick(kJoystickPort);
+
+  @Override
+  public void robotInit() {
+    // We go 2 pi radians per 4096 clicks.
+    m_encoder.setDistancePerPulse(
+          2.0 * Math.PI / 4096.0);
+  }
+
+  @Override
+  public void teleopInit() {
+    // Reset our loop to make sure it's in a known state.
+    m_loop.reset(VecBuilder.fill(m_encoder.getRate()));
+  }
+
+  @Override
+  public void teleopPeriodic() {
+
+    // Sets the target speed of our flywheel. This is similar to setting the setpoint of a
+    // PID controller.
+    if (m_joystick.getTriggerPressed()) {
+      // We just pressed the trigger, so let's set our next reference
+      m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
+    } else if (m_joystick.getTriggerReleased()) {
+      // We just released the trigger, so let's spin down
+      m_loop.setNextR(VecBuilder.fill(0.0));
+    }
+
+    // Correct our Kalman filter's state vector estimate with encoder data.
+    m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
+
+    // Update our LQR to generate new voltage commands and use the voltages to predict the next
+    // state with out Kalman filter.
+    m_loop.predict(0.020);
+
+    // Send the new calculated voltage to the motors.
+    // voltage = duty cycle * battery voltage, so
+    // duty cycle = voltage / battery voltage
+    double nextVoltage = m_loop.getU(0);
+    m_motor.setVoltage(nextVoltage);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index 828a94b..5f1227a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,6 @@
 package edu.wpi.first.wpilibj.examples.swervebot;
 
 import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
@@ -37,23 +36,14 @@
       m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
   );
 
-  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, getAngle());
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+      m_gyro.getRotation2d());
 
   public Drivetrain() {
     m_gyro.reset();
   }
 
   /**
-   * Returns the angle of the robot as a Rotation2d.
-   *
-   * @return The angle of the robot.
-   */
-  public Rotation2d getAngle() {
-    // Negating the angle because WPILib gyros are CW positive.
-    return Rotation2d.fromDegrees(-m_gyro.getAngle());
-  }
-
-  /**
    * Method to drive the robot using joystick info.
    *
    * @param xSpeed        Speed of the robot in the x direction (forward).
@@ -65,7 +55,7 @@
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
     var swerveModuleStates = m_kinematics.toSwerveModuleStates(
         fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            xSpeed, ySpeed, rot, getAngle())
+            xSpeed, ySpeed, rot, m_gyro.getRotation2d())
             : new ChassisSpeeds(xSpeed, ySpeed, rot)
     );
     SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
@@ -80,7 +70,7 @@
    */
   public void updateOdometry() {
     m_odometry.update(
-        getAngle(),
+        m_gyro.getRotation2d(),
         m_frontLeft.getState(),
         m_frontRight.getState(),
         m_backLeft.getState(),

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
index 35afdf3..b8c4b8f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.examples.swervebot;
 
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
@@ -15,6 +16,11 @@
   private final XboxController m_controller = new XboxController(0);
   private final Drivetrain m_swerve = new Drivetrain();
 
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
   @Override
   public void autonomousPeriodic() {
     driveWithJoystick(false);
@@ -29,18 +35,24 @@
   private void driveWithJoystick(boolean fieldRelative) {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
+    final var xSpeed =
+        -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
+            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
-    final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * Drivetrain.kMaxSpeed;
+    final var ySpeed =
+        -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
+            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed;
+    final var rot =
+        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
+            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
 
     m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index 2969fd4..e13108f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
@@ -36,6 +37,10 @@
       = new ProfiledPIDController(1, 0, 0,
       new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
 
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
+  private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
+
   /**
    * Constructs a SwerveModule.
    *
@@ -77,16 +82,20 @@
    */
   public void setDesiredState(SwerveModuleState state) {
     // Calculate the drive output from the drive PID controller.
-    final var driveOutput = m_drivePIDController.calculate(
+    final double driveOutput = m_drivePIDController.calculate(
         m_driveEncoder.getRate(), state.speedMetersPerSecond);
 
+    final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
+
     // Calculate the turning motor output from the turning PID controller.
-    final var turnOutput = m_turningPIDController.calculate(
+    final double turnOutput = m_turningPIDController.calculate(
         m_turningEncoder.get(), state.angle.getRadians()
     );
 
-    // Calculate the turning motor output from the turning PID controller.
-    m_driveMotor.set(driveOutput);
-    m_turningMotor.set(turnOutput);
+    final double turnFeedforward =
+        m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
+
+    m_driveMotor.setVoltage(driveOutput + driveFeedforward);
+    m_turningMotor.setVoltage(turnOutput + turnFeedforward);
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
index 503cede..544cb1a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -98,6 +98,10 @@
         config
     );
 
+    var thetaController = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
+        AutoConstants.kThetaControllerConstraints);
+    thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
     SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
         exampleTrajectory,
         m_robotDrive::getPose, //Functional interface to feed supplier
@@ -105,9 +109,7 @@
 
         //Position controllers
         new PIDController(AutoConstants.kPXController, 0, 0),
-        new PIDController(AutoConstants.kPYController, 0, 0),
-        new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
-                                  AutoConstants.kThetaControllerConstraints),
+        new PIDController(AutoConstants.kPYController, 0, 0), thetaController,
 
         m_robotDrive::setModuleStates,
 
@@ -115,6 +117,9 @@
 
     );
 
+    // Reset odometry to the starting pose of the trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
index 5d9cf56..2baeec4 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -60,7 +60,7 @@
 
   // Odometry class for tracking robot pose
   SwerveDriveOdometry m_odometry =
-      new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getAngle());
+      new SwerveDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
 
   /**
    * Creates a new DriveSubsystem.
@@ -68,16 +68,6 @@
   public DriveSubsystem() {
   }
 
-  /**
-   * Returns the angle of the robot as a Rotation2d.
-   *
-   * @return The angle of the robot.
-   */
-  public Rotation2d getAngle() {
-    // Negating the angle because WPILib gyros are CW positive.
-    return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? 1.0 : -1.0));
-  }
-
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
@@ -104,7 +94,7 @@
    * @param pose The pose to which to set the odometry.
    */
   public void resetOdometry(Pose2d pose) {
-    m_odometry.resetPosition(pose, getAngle());
+    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
   }
 
   /**
@@ -119,7 +109,7 @@
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
     var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
         fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            xSpeed, ySpeed, rot, getAngle())
+            xSpeed, ySpeed, rot, m_gyro.getRotation2d())
             : new ChassisSpeeds(xSpeed, ySpeed, rot)
     );
     SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates,
@@ -164,10 +154,10 @@
   /**
    * Returns the heading of the robot.
    *
-   * @return the robot's heading in degrees, from 180 to 180
+   * @return the robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
-    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    return m_gyro.getRotation2d().getDegrees();
   }
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
index 8d56fcf..76202fe 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -21,4 +21,9 @@
   public void periodic() {
     // This method will be called once per scheduler run
   }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
index 783f76b..bfd3c46 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -55,7 +55,7 @@
         m_ds.InAutonomous(true);
         autonomous();
         m_ds.InAutonomous(false);
-        while (isAutonomous() && !isDisabled()) {
+        while (isAutonomousEnabled()) {
           m_ds.waitForData();
         }
       } else if (isTest()) {
@@ -73,7 +73,7 @@
         m_ds.InOperatorControl(true);
         teleop();
         m_ds.InOperatorControl(false);
-        while (isOperatorControl() && !isDisabled()) {
+        while (isOperatorControlEnabled()) {
           m_ds.waitForData();
         }
       }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
index 9930de7..5974b41 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -82,6 +82,13 @@
   }
 
   /**
+   * This function is called once when teleop is enabled.
+   */
+  @Override
+  public void teleopInit() {
+  }
+
+  /**
    * This function is called periodically during operator control.
    */
   @Override
@@ -89,6 +96,27 @@
   }
 
   /**
+   * This function is called once when the robot is disabled.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  /**
+   * This function is called periodically when disabled.
+   */
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This function is called once when test mode is enabled.
+   */
+  @Override
+  public void testInit() {
+  }
+
+  /**
    * This function is called periodically during test mode.
    */
   @Override

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
index 9ec7991..b4480c8 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -26,6 +26,10 @@
   }
 
   @Override
+  public void robotPeriodic() {
+  }
+
+  @Override
   public void autonomousInit() {
   }
 
@@ -42,6 +46,14 @@
   }
 
   @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  @Override
   public void testInit() {
   }
 

wpilibjIntegrationTests/build.gradle

diff --git a/wpilibjIntegrationTests/build.gradle b/wpilibjIntegrationTests/build.gradle
index ec5714e..090d309 100644
--- a/wpilibjIntegrationTests/build.gradle
+++ b/wpilibjIntegrationTests/build.gradle
@@ -21,6 +21,7 @@
 
 dependencies {
     implementation project(':wpilibj')
+    implementation project(':wpimath')
     implementation project(':hal')
     implementation project(':wpiutil')
     implementation project(':ntcore')

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java

diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
index 7fcd01f..e6e48a6 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -177,47 +177,45 @@
 
   @Test
   public void testPositionPIDController() {
-    PIDController pidController = new PIDController(0.001, 0.0005, 0);
-    pidController.setTolerance(50.0);
-    pidController.setIntegratorRange(-0.2, 0.2);
-    pidController.setSetpoint(1000);
+    try (PIDController pidController = new PIDController(0.001, 0.0005, 0)) {
+      pidController.setTolerance(50.0);
+      pidController.setIntegratorRange(-0.2, 0.2);
+      pidController.setSetpoint(1000);
 
-    Notifier pidRunner = new Notifier(() -> {
-      var speed = pidController.calculate(me.getEncoder().getDistance());
-      me.getMotor().set(MathUtil.clamp(speed, -0.2, 0.2));
-    });
+      try (Notifier pidRunner = new Notifier(() -> {
+        var speed = pidController.calculate(me.getEncoder().getDistance());
+        me.getMotor().set(MathUtil.clamp(speed, -0.2, 0.2));
+      })) {
+        pidRunner.startPeriodic(pidController.getPeriod());
+        Timer.delay(10.0);
+        pidRunner.stop();
 
-    pidRunner.startPeriodic(pidController.getPeriod());
-    Timer.delay(10.0);
-    pidRunner.stop();
-
-    assertTrue(
-        "PID loop did not reach reference within 10 seconds. The current error was" + pidController
-            .getPositionError(), pidController.atSetpoint());
-
-    pidRunner.close();
+        assertTrue(
+            "PID loop did not reach reference within 10 seconds. The current error was"
+            + pidController.getPositionError(), pidController.atSetpoint());
+      }
+    }
   }
 
   @Test
   public void testVelocityPIDController() {
     LinearFilter filter = LinearFilter.movingAverage(50);
-    PIDController pidController = new PIDController(1e-5, 0.0, 0.0006);
-    pidController.setTolerance(200);
-    pidController.setSetpoint(600);
+    try (PIDController pidController = new PIDController(1e-5, 0.0, 0.0006)) {
+      pidController.setTolerance(200);
+      pidController.setSetpoint(600);
 
-    Notifier pidRunner = new Notifier(() -> {
-      var speed = filter.calculate(me.getEncoder().getRate());
-      me.getMotor().set(MathUtil.clamp(speed, -0.3, 0.3));
-    });
+      try (Notifier pidRunner = new Notifier(() -> {
+        var speed = filter.calculate(me.getEncoder().getRate());
+        me.getMotor().set(MathUtil.clamp(speed, -0.3, 0.3));
+      })) {
+        pidRunner.startPeriodic(pidController.getPeriod());
+        Timer.delay(10.0);
+        pidRunner.stop();
 
-    pidRunner.startPeriodic(pidController.getPeriod());
-    Timer.delay(10.0);
-    pidRunner.stop();
-
-    assertTrue("PID loop did not reach reference within 10 seconds. The error was: " + pidController
-        .getPositionError(), pidController.atSetpoint());
-
-    pidRunner.close();
+        assertTrue("PID loop did not reach reference within 10 seconds. The error was: "
+            + pidController.getPositionError(), pidController.atSetpoint());
+      }
+    }
   }
 
   /**

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java

diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java
index ade9e69..fa17baa 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -78,7 +78,7 @@
 
   @Override
   public boolean teardown() {
-    logger.log(Level.FINE, "Begining teardown");
+    logger.log(Level.FINE, "Beginning teardown");
     if (m_allocated) {
       m_input.close();
       m_output.close();

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java

diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java
index a944421..8dbb4d3 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -98,7 +98,7 @@
    */
   @Override
   public boolean teardown() {
-    logger.log(Level.FINE, "Begining teardown");
+    logger.log(Level.FINE, "Beginning teardown");
     m_counter.close();
     m_source.close();
     if (m_allocated) { // Only tear down the dio if this class allocated it

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java

diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java
index 2b2b852..ef9da0e 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -100,7 +100,7 @@
    */
   @Override
   public boolean teardown() {
-    logger.fine("Begining teardown");
+    logger.fine("Beginning teardown");
     m_source.close();
     logger.finer("Source freed " + m_sourcePort[0] + ",  " + m_sourcePort[1]);
     m_encoder.close();

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java

diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java
index df797f1..3465915 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,7 @@
 import edu.wpi.first.wpilibj.test.TestBench;
 
 /**
- * Master interface for all test fixtures. This ensures that all test fixtures have setup and
+ * Common interface for all test fixtures. This ensures that all test fixtures have setup and
  * teardown methods, to ensure that the tests run properly. Test fixtures should be modeled around
  * the content of a test, rather than the actual physical layout of the testing board. They should
  * obtain references to hardware from the {@link TestBench} class, which is a singleton. Testing

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java

diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
index e4b2035..54aae9e 100644
--- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
+++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -116,7 +116,7 @@
             | IllegalAccessException ex) {
           // This shouldn't happen unless the constructor is changed in some
           // way.
-          logger.log(Level.SEVERE, "Test suites can not take paramaters in their constructors.",
+          logger.log(Level.SEVERE, "Test suites can not take parameters in their constructors.",
               ex);
         }
       } else if (c.getAnnotation(SuiteClasses.class) != null) {
@@ -212,7 +212,7 @@
             | IllegalAccessException ex) {
           // This shouldn't happen unless the constructor is changed in some
           // way.
-          logger.log(Level.SEVERE, "Test suites can not take paramaters in their constructors.",
+          logger.log(Level.SEVERE, "Test suites can not take parameters in their constructors.",
               ex);
         }
       }

wpilibjIntegrationTests/src/main/resources/logging.properties

diff --git a/wpilibjIntegrationTests/src/main/resources/logging.properties b/wpilibjIntegrationTests/src/main/resources/logging.properties
index b15c534..33ad75f 100644
--- a/wpilibjIntegrationTests/src/main/resources/logging.properties
+++ b/wpilibjIntegrationTests/src/main/resources/logging.properties
@@ -6,7 +6,7 @@
 # Default global logging level.
 # This specifies which kinds of events are logged across
 # all loggers.  For any given facility this global level
-# can be overriden by a facility specific level
+# can be overridden by a facility specific level
 # Note that the ConsoleHandler also has a separate level
 # setting to limit messages printed to the console.
 #.level= INFO

wpimath/.styleguide

diff --git a/wpimath/.styleguide b/wpimath/.styleguide
new file mode 100644
index 0000000..c82141b
--- /dev/null
+++ b/wpimath/.styleguide
@@ -0,0 +1,42 @@
+cppHeaderFileInclude {
+  \.h$
+  \.inc$
+  \.inl$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+generatedFileExclude {
+  src/main/native/cpp/drake/
+  src/main/native/eigeninclude/
+  src/main/native/include/drake/
+  src/main/native/include/units/base\.h$
+  src/main/native/include/units/units\.h$
+  src/main/native/include/unsupported/
+  src/test/native/cpp/UnitsTest\.cpp$
+  src/test/native/cpp/drake/
+  src/test/native/include/drake/
+}
+
+repoRootNameOverride {
+  wpimath
+}
+
+includeGuardRoots {
+  wpimath/src/main/native/cpp/
+  wpimath/src/main/native/include/
+  wpimath/src/test/native/cpp/
+}
+
+includeOtherLibs {
+  ^wpi/
+}
+
+includeProject {
+  ^drake/
+  ^Eigen/
+  ^units/
+  ^unsupported/
+}

wpimath/CMakeLists.txt

diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt
new file mode 100644
index 0000000..a129376
--- /dev/null
+++ b/wpimath/CMakeLists.txt
@@ -0,0 +1,142 @@
+project(wpimath)
+
+include(SubDirList)
+include(CompileWarnings)
+include(AddTest)
+
+file(GLOB wpimath_jni_src src/main/native/cpp/jni/WPIMathJNI.cpp)
+
+# Java bindings
+if (WITH_JAVA)
+  find_package(Java REQUIRED)
+  find_package(JNI REQUIRED)
+  include(UseJava)
+  set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+
+  if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.38.jar")
+      set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
+      set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml")
+
+      message(STATUS "Downloading EJML jarfiles...")
+
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-cdense/0.38/ejml-cdense-0.38.jar"
+          "${JAR_ROOT}/ejml-cdense-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-core/0.38/ejml-core-0.38.jar"
+          "${JAR_ROOT}/ejml-core-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-ddense/0.38/ejml-ddense-0.38.jar"
+          "${JAR_ROOT}/ejml-ddense-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-dsparse/0.38/ejml-dsparse-0.38.jar"
+          "${JAR_ROOT}/ejml-dsparse-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-fdense/0.38/ejml-fdense-0.38.jar"
+          "${JAR_ROOT}/ejml-fdense-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-simple/0.38/ejml-simple-0.38.jar"
+          "${JAR_ROOT}/ejml-simple-0.38.jar")
+      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-zdense/0.38/ejml-zdense-0.38.jar"
+          "${JAR_ROOT}/ejml-zdense-0.38.jar")
+
+      message(STATUS "All files downloaded.")
+  endif()
+
+  file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+  file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+
+  set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS})
+
+  execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpimath/generate_numbers.py ${CMAKE_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
+  if(NOT (generateResult EQUAL "0"))
+    # Try python
+    execute_process(COMMAND python ${CMAKE_SOURCE_DIR}/wpimath/generate_numbers.py ${CMAKE_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
+    if(NOT (generateResult EQUAL "0"))
+      message(FATAL_ERROR "python and python3 generate_numbers.py failed")
+    endif()
+  endif()
+
+  set(CMAKE_JNI_TARGET true)
+
+  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${CMAKE_BINARY_DIR}/wpimath/generated/*.java)
+
+  if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+    set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
+    add_jar(wpimath_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath)
+  else()
+    add_jar(wpimath_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers)
+  endif()
+
+  get_property(WPIMATH_JAR_FILE TARGET wpimath_jar PROPERTY JAR_FILE)
+  install(FILES ${WPIMATH_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+  set_property(TARGET wpimath_jar PROPERTY FOLDER "java")
+
+  add_library(wpimathjni ${wpimath_jni_src})
+  wpilib_target_warnings(wpimathjni)
+  target_link_libraries(wpimathjni PUBLIC wpimath)
+
+  set_property(TARGET wpimathjni PROPERTY FOLDER "libraries")
+
+  if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+    target_include_directories(wpimathjni PRIVATE ${JNI_INCLUDE_DIRS})
+    target_include_directories(wpimathjni PRIVATE "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
+  else()
+    target_link_libraries(wpimathjni PRIVATE wpimath_jni_headers)
+  endif()
+  add_dependencies(wpimathjni wpimath_jar)
+
+  if (MSVC)
+    install(TARGETS wpimathjni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+  endif()
+
+  install(TARGETS wpimathjni EXPORT wpimathjni DESTINATION "${main_lib_dest}")
+
+endif()
+
+file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp)
+list(REMOVE_ITEM wpimath_native_src ${wpimath_jni_src})
+
+add_library(wpimath ${wpimath_native_src})
+set_target_properties(wpimath PROPERTIES DEBUG_POSTFIX "d")
+
+set_property(TARGET wpimath PROPERTY FOLDER "libraries")
+
+target_compile_features(wpimath PUBLIC cxx_std_17)
+if (MSVC)
+    target_compile_options(wpimath PUBLIC /bigobj)
+endif()
+wpilib_target_warnings(wpimath)
+target_link_libraries(wpimath wpiutil)
+
+if (NOT USE_VCPKG_EIGEN)
+    install(DIRECTORY src/main/native/eigeninclude/ DESTINATION "${include_dest}/wpimath")
+    target_include_directories(wpimath PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/eigeninclude>
+                            $<INSTALL_INTERFACE:${include_dest}/wpimath>)
+else()
+    find_package(Eigen3 CONFIG REQUIRED)
+    target_link_libraries (wpimath Eigen3::Eigen)
+endif()
+
+target_include_directories(wpimath PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<INSTALL_INTERFACE:${include_dest}/wpimath>)
+
+install(TARGETS wpimath EXPORT wpimath DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpimath")
+
+if (WITH_JAVA AND MSVC)
+    install(TARGETS wpimath RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+endif()
+
+if (WITH_FLAT_INSTALL)
+    set (wpimath_config_dir ${wpilib_dest})
+else()
+    set (wpimath_config_dir share/wpimath)
+endif()
+
+configure_file(wpimath-config.cmake.in ${CMAKE_BINARY_DIR}/wpimath-config.cmake )
+install(FILES ${CMAKE_BINARY_DIR}/wpimath-config.cmake DESTINATION ${wpimath_config_dir})
+install(EXPORT wpimath DESTINATION ${wpimath_config_dir})
+
+if (WITH_TESTS)
+    wpilib_add_test(wpimath src/test/native/cpp)
+    target_include_directories(wpimath_test PRIVATE src/test/native/include)
+    target_link_libraries(wpimath_test wpimath gmock_main)
+endif()

wpimath/build.gradle

diff --git a/wpimath/build.gradle b/wpimath/build.gradle
new file mode 100644
index 0000000..c023a1a
--- /dev/null
+++ b/wpimath/build.gradle
@@ -0,0 +1,109 @@
+ext {
+    useJava = true
+    useCpp = true
+    baseId = 'wpimath'
+    groupId = 'edu.wpi.first.wpimath'
+
+    nativeName = 'wpimath'
+    devMain = 'edu.wpi.first.wpiutil.math.DevMain'
+}
+
+apply from: "${rootDir}/shared/jni/setupBuild.gradle"
+
+nativeUtils.exportsConfigs {
+    wpimath {
+        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+    }
+}
+
+cppHeadersZip {
+    from('src/main/native/eigeninclude') {
+        into '/'
+    }
+}
+
+model {
+    components {
+        all {
+            it.sources.each {
+                it.exportedHeaders {
+                    srcDirs 'src/main/native/include', 'src/main/native/eigeninclude'
+                }
+            }
+        }
+    }
+}
+
+dependencies {
+    api "org.ejml:ejml-simple:0.38"
+    api "com.fasterxml.jackson.core:jackson-annotations:2.10.0"
+    api "com.fasterxml.jackson.core:jackson-core:2.10.0"
+    api "com.fasterxml.jackson.core:jackson-databind:2.10.0"
+}
+
+def wpilibNumberFileInput = file("src/generate/GenericNumber.java.in")
+def natFileInput = file("src/generate/Nat.java.in")
+def natGetterInput = file("src/generate/NatGetter.java.in")
+def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/numbers")
+def wpilibNatFileOutput = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/Nat.java")
+def maxNum = 20
+
+task generateNumbers() {
+    description = "Generates generic number classes from template"
+    group = "WPILib"
+
+    inputs.file wpilibNumberFileInput
+    outputs.dir wpilibNumberFileOutputDir
+
+    doLast {
+        if(wpilibNumberFileOutputDir.exists()) {
+            wpilibNumberFileOutputDir.delete()
+        }
+        wpilibNumberFileOutputDir.mkdirs()
+
+        for(i in 0..maxNum) {
+            def outputFile = new File(wpilibNumberFileOutputDir, "N${i}.java")
+            def read = wpilibNumberFileInput.text.replace('${num}', i.toString())
+            outputFile.write(read)
+        }
+    }
+}
+
+task generateNat() {
+    description = "Generates Nat.java"
+    group = "WPILib"
+    inputs.files([natFileInput, natGetterInput])
+    outputs.file wpilibNatFileOutput
+    dependsOn generateNumbers
+
+    doLast {
+        if(wpilibNatFileOutput.exists()) {
+            wpilibNatFileOutput.delete()
+        }
+
+        def template = natFileInput.text + "\n"
+
+        def importsString = "";
+
+        for(i in 0..maxNum) {
+            importsString += "import edu.wpi.first.wpiutil.math.numbers.N${i};\n"
+            template += natGetterInput.text.replace('${num}', i.toString()) + "\n"
+        }
+        template += "}\n" // Close the class body
+
+        template = template.replace('{{REPLACEWITHIMPORTS}}', importsString)
+
+        wpilibNatFileOutput.write(template)
+    }
+}
+
+sourceSets.main.java.srcDir "${buildDir}/generated/java"
+compileJava.dependsOn generateNumbers
+compileJava.dependsOn generateNat

wpimath/generate_numbers.py

diff --git a/wpimath/generate_numbers.py b/wpimath/generate_numbers.py
new file mode 100644
index 0000000..701f3e6
--- /dev/null
+++ b/wpimath/generate_numbers.py
@@ -0,0 +1,59 @@
+import os
+import sys
+
+
+def main():
+    MAX_NUM = 20
+
+    dirname, _ = os.path.split(os.path.abspath(__file__))
+    cmake_binary_dir = sys.argv[1]
+
+    with open(f"{dirname}/src/generate/GenericNumber.java.in",
+              "r") as templateFile:
+        template = templateFile.read()
+        rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/numbers"
+
+        if not os.path.exists(rootPath):
+            os.makedirs(rootPath)
+
+        for i in range(MAX_NUM + 1):
+            new_contents = template.replace("${num}", str(i))
+
+            if os.path.exists(f"{rootPath}/N{i}.java"):
+                with open(f"{rootPath}/N{i}.java", "r") as f:
+                    if f.read() == new_contents:
+                        continue
+
+            # File either doesn't exist or has different contents
+            with open(f"{rootPath}/N{i}.java", "w") as f:
+                f.write(new_contents)
+
+    with open(f"{dirname}/src/generate/Nat.java.in", "r") as templateFile:
+        template = templateFile.read()
+        outputPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/Nat.java"
+        with open(f"{dirname}/src/generate/NatGetter.java.in",
+                  "r") as getterFile:
+            getter = getterFile.read()
+
+        importsString = ""
+
+        for i in range(MAX_NUM + 1):
+            importsString += f"import edu.wpi.first.wpiutil.math.numbers.N{i};\n"
+            template += getter.replace("${num}", str(i))
+
+        template += "}\n"
+
+        template = template.replace('{{REPLACEWITHIMPORTS}}', importsString)
+
+        if os.path.exists(outputPath):
+            with open(outputPath, "r") as f:
+                if f.read() == template:
+                    return 0
+
+        # File either doesn't exist or has different contents
+        with open(outputPath, "w") as f:
+            f.write(template)
+
+
+if __name__ == "__main__":
+    main()

wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java

diff --git a/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java b/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java
new file mode 100644
index 0000000..9a5d378
--- /dev/null
+++ b/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+public final class DevMain {
+  /**
+   * Main entry point.
+   */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    System.out.println(MathUtil.normalizeAngle(-5.0));
+  }
+
+  private DevMain() {
+  }
+}

wpimath/src/dev/native/cpp/main.cpp

diff --git a/wpimath/src/dev/native/cpp/main.cpp b/wpimath/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..030c8f9
--- /dev/null
+++ b/wpimath/src/dev/native/cpp/main.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+
+#include <wpi/math>
+
+int main() { std::cout << wpi::math::pi << std::endl; }

wpimath/src/generate/GenericNumber.java.in

diff --git a/wpiutil/src/generate/GenericNumber.java.in b/wpimath/src/generate/GenericNumber.java.in
similarity index 100%
rename from wpiutil/src/generate/GenericNumber.java.in
rename to wpimath/src/generate/GenericNumber.java.in

wpimath/src/generate/Nat.java.in

diff --git a/wpimath/src/generate/Nat.java.in b/wpimath/src/generate/Nat.java.in
new file mode 100644
index 0000000..666bd1c
--- /dev/null
+++ b/wpimath/src/generate/Nat.java.in
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+//CHECKSTYLE.OFF: ImportOrder
+{{REPLACEWITHIMPORTS}}
+//CHECKSTYLE.ON
+
+/**
+ * A natural number expressed as a java class.
+ * The counterpart to {@link Num} that should be used as a concrete value.
+ *
+ * @param <T> The {@link Num} this represents.
+ */
+@SuppressWarnings({"MethodName", "unused"})
+public interface Nat<T extends Num> {
+  /**
+   * The number this interface represents.
+   *
+   * @return The number backing this value.
+   */
+  int getNum();

wpimath/src/generate/NatGetter.java.in

diff --git a/wpiutil/src/generate/NatGetter.java.in b/wpimath/src/generate/NatGetter.java.in
similarity index 100%
rename from wpiutil/src/generate/NatGetter.java.in
rename to wpimath/src/generate/NatGetter.java.in

wpimath/src/main/java/edu/wpi/first/math/Drake.java

diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
new file mode 100644
index 0000000..4d66102
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+
+public final class Drake {
+  private Drake() {
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
+          SimpleMatrix A,
+          SimpleMatrix B,
+          SimpleMatrix Q,
+          SimpleMatrix R) {
+    var S = new SimpleMatrix(A.numRows(), A.numCols());
+    WPIMathJNI.discreteAlgebraicRiccatiEquation(A.getDDRM().getData(), B.getDDRM().getData(),
+            Q.getDDRM().getData(), R.getDDRM().getData(), A.numCols(), B.numCols(),
+            S.getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num, Inputs extends Num> Matrix<States, States>
+      discreteAlgebraicRiccatiEquation(Matrix<States, States> A,
+                                       Matrix<States, Inputs> B,
+                                       Matrix<States, States> Q,
+                                       Matrix<Inputs, Inputs> R) {
+    return new Matrix<>(discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(),
+    Q.getStorage(), R.getStorage()));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/MathShared.java

diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathShared.java b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
new file mode 100644
index 0000000..168dbb5
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public interface MathShared {
+  /**
+   * Report an error.
+   *
+   * @param error the error to set
+   */
+  void reportError(String error, StackTraceElement[] stackTrace);
+
+  /**
+   * Report usage.
+   *
+   * @param id the usage id
+   * @param count the usage count
+   */
+  void reportUsage(MathUsageId id, int count);
+}

wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java

diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
new file mode 100644
index 0000000..a4c8425
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public final class MathSharedStore {
+  private static MathShared mathShared;
+
+  private MathSharedStore() {
+  }
+
+  /**
+   * get the MathShared object.
+   */
+  public static synchronized MathShared getMathShared() {
+    if (mathShared == null) {
+      mathShared = new MathShared() {
+        @Override
+        public void reportError(String error, StackTraceElement[] stackTrace) {
+        }
+
+        @Override
+        public void reportUsage(MathUsageId id, int count) {
+        }
+      };
+    }
+    return mathShared;
+  }
+
+  /**
+   * set the MathShared object.
+   */
+  public static synchronized void setMathShared(MathShared shared) {
+    mathShared = shared;
+  }
+
+  /**
+   * Report an error.
+   *
+   * @param error the error to set
+   */
+  public static void reportError(String error, StackTraceElement[] stackTrace) {
+    getMathShared().reportError(error, stackTrace);
+  }
+
+  /**
+   * Report usage.
+   *
+   * @param id the usage id
+   * @param count the usage count
+   */
+  public static void reportUsage(MathUsageId id, int count) {
+    getMathShared().reportUsage(id, count);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java

diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java b/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
new file mode 100644
index 0000000..ed95e24
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public enum MathUsageId {
+  kKinematics_DifferentialDrive,
+  kKinematics_MecanumDrive,
+  kKinematics_SwerveDrive,
+  kTrajectory_TrapezoidProfile,
+  kFilter_Linear,
+  kOdometry_DifferentialDrive,
+  kOdometry_SwerveDrive,
+  kOdometry_MecanumDrive
+}

wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
new file mode 100644
index 0000000..30984ac
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+import edu.wpi.first.wpiutil.RuntimeLoader;
+
+public final class WPIMathJNI {
+  static boolean libraryLoaded = false;
+  static RuntimeLoader<WPIMathJNI> loader = null;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
+                WPIMathJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
+            WPIMathJNI.class);
+    loader.loadLibrary();
+    libraryLoaded = true;
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
+   * @param A      Array containing elements of A in row-major order.
+   * @param B      Array containing elements of B in row-major order.
+   * @param Q      Array containing elements of Q in row-major order.
+   * @param R      Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S      Array storage for DARE solution.
+   */
+  public static native void discreteAlgebraicRiccatiEquation(
+          double[] A,
+          double[] B,
+          double[] Q,
+          double[] R,
+          int states,
+          int inputs,
+          double[] S);
+
+  /**
+   * Computes the matrix exp.
+   *
+   * @param src  Array of elements of the matrix to be exponentiated.
+   * @param rows how many rows there are.
+   * @param dst  Array where the result will be stored.
+   */
+  public static native void exp(double[] src, int rows, double[] dst);
+
+  /**
+   * Returns true if (A, B) is a stabilizable pair.
+   *
+   * <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+   * any, have absolute values less than one, where an eigenvalue is
+   * uncontrollable if rank(lambda * I - A, B) &lt; n where n is number of states.
+   *
+   * @param states the number of states of the system.
+   * @param inputs the number of inputs to the system.
+   * @param A      System matrix.
+   * @param B      Input matrix.
+   * @return If the system is stabilizable.
+   */
+  public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
+
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
new file mode 100644
index 0000000..10897e8
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
@@ -0,0 +1,171 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Arrays;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
+ * Static factory methods are provided to create commonly used types of filters.
+ *
+ * <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
+ * a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
+ * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
+ * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
+ * front of the feedback term! This is a common convention in signal processing.
+ *
+ * <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
+ * undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
+ * noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
+ * impact of these high frequency components.  Likewise, a "high pass" filter gets rid of
+ * slow-moving signal components, letting you detect large changes more easily.
+ *
+ * <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
+ * the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
+ * wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
+ * strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
+ * PID controller out of this class!
+ *
+ * <p>For more on filters, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * <p>Note 1: calculate() should be called by the user on a known, regular period. You can use a
+ * Notifier for this or do it "inline" with code in a periodic function.
+ *
+ * <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
+ * that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
+ * then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
+ * to make sure calculate() gets called at the desired, constant frequency!
+ */
+public class LinearFilter {
+  private final CircularBuffer m_inputs;
+  private final CircularBuffer m_outputs;
+  private final double[] m_inputGains;
+  private final double[] m_outputGains;
+
+  private static int instances;
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feed forward" or FIR gains.
+   * @param fbGains The "feed back" or IIR gains.
+   */
+  public LinearFilter(double[] ffGains, double[] fbGains) {
+    m_inputs = new CircularBuffer(ffGains.length);
+    m_outputs = new CircularBuffer(fbGains.length);
+    m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+    instances++;
+    MathSharedStore.reportUsage(MathUsageId.kFilter_Linear, instances);
+  }
+
+  /**
+   * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
+   * gain = e^(-dt / T), T is the time constant in seconds.
+   *
+   * <p>This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the user.
+   */
+  public static LinearFilter singlePoleIIR(double timeConstant,
+                                           double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {1.0 - gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
+   * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
+   *
+   * <p>This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the user.
+   */
+  public static LinearFilter highPass(double timeConstant,
+                                      double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {gain, -gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
+   * x[0]).
+   *
+   * <p>This filter is always stable.
+   *
+   * @param taps The number of samples to average over. Higher = smoother but slower.
+   * @throws IllegalArgumentException if number of taps is less than 1.
+   */
+  public static LinearFilter movingAverage(int taps) {
+    if (taps <= 0) {
+      throw new IllegalArgumentException("Number of taps was not at least 1");
+    }
+
+    double[] ffGains = new double[taps];
+    for (int i = 0; i < ffGains.length; i++) {
+      ffGains[i] = 1.0 / taps;
+    }
+
+    double[] fbGains = new double[0];
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Reset the filter state.
+   */
+  public void reset() {
+    m_inputs.clear();
+    m_outputs.clear();
+  }
+
+  /**
+   * Calculates the next value of the filter.
+   *
+   * @param input Current input value.
+   *
+   * @return The filtered value at this step
+   */
+  public double calculate(double input) {
+    double retVal = 0.0;
+
+    // Rotate the inputs
+    m_inputs.addFirst(input);
+
+    // Calculate the new value
+    for (int i = 0; i < m_inputGains.length; i++) {
+      retVal += m_inputs.get(i) * m_inputGains[i];
+    }
+    for (int i = 0; i < m_outputGains.length; i++) {
+      retVal -= m_outputs.get(i) * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    m_outputs.addFirst(retVal);
+
+    return retVal;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
new file mode 100644
index 0000000..18998b0
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+import edu.wpi.first.wpiutil.CircularBuffer;
+
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing measurement noise,
+ * especially with processes that generate occasional, extreme outliers (such as values from
+ * vision processing, LIDAR, or ultrasonic sensors).
+ */
+public class MedianFilter {
+  private final CircularBuffer m_valueBuffer;
+  private final List<Double> m_orderedValues;
+  private final int m_size;
+
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  public MedianFilter(int size) {
+    // Circular buffer of values currently in the window, ordered by time
+    m_valueBuffer = new CircularBuffer(size);
+    // List of values currently in the window, ordered by value
+    m_orderedValues = new ArrayList<>(size);
+    // Size of rolling window
+    m_size = size;
+  }
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  public double calculate(double next) {
+    // Find insertion point for next value
+    int index = Collections.binarySearch(m_orderedValues, next);
+
+    // Deal with binarySearch behavior for element not found
+    if (index < 0) {
+      index = Math.abs(index + 1);
+    }
+
+    // Place value at proper insertion point
+    m_orderedValues.add(index, next);
+
+    int curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.remove(m_valueBuffer.removeLast());
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.addFirst(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues.get(curSize / 2);
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
+    }
+  }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  public void reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.clear();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
new file mode 100644
index 0000000..59e927a
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
@@ -0,0 +1,144 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple arm (modeled as a motor
+ * acting against the force of gravity on a beam suspended at an angle).
+ */
+@SuppressWarnings("MemberName")
+public class ArmFeedforward {
+  public final double ks;
+  public final double kcos;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains.  Units of the gain values
+   * will dictate units of the computed feedforward.
+   *
+   * @param ks   The static gain.
+   * @param kcos The gravity gain.
+   * @param kv   The velocity gain.
+   * @param ka   The acceleration gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv, double ka) {
+    this.ks = ks;
+    this.kcos = kcos;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains.  Acceleration gain is
+   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks   The static gain.
+   * @param kcos The gravity gain.
+   * @param kv   The velocity gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv) {
+    this(ks, kcos, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param positionRadians       The position (angle) setpoint.
+   * @param velocityRadPerSec     The velocity setpoint.
+   * @param accelRadPerSecSquared The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double positionRadians, double velocityRadPerSec,
+                          double accelRadPerSecSquared) {
+    return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians)
+        + kv * velocityRadPerSec
+        + ka * accelRadPerSecSquared;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+   * be zero).
+   *
+   * @param positionRadians The position (angle) setpoint.
+   * @param velocity        The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double positionRadians, double velocity) {
+    return calculate(positionRadians, velocity, 0);
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param acceleration The acceleration of the arm.
+   * @return The maximum possible velocity at the given acceleration and angle.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param acceleration The acceleration of the arm.
+   * @return The minimum possible velocity at the given acceleration and angle.
+   */
+  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply, a position, 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param velocity The velocity of the arm.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply, a position, 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm.
+   * @param velocity The velocity of the arm.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java
new file mode 100644
index 0000000..79a88cd
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java
@@ -0,0 +1,215 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import java.util.function.BiFunction;
+import java.util.function.Function;
+
+import edu.wpi.first.wpilibj.system.NumericalJacobian;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * Constructs a control-affine plant inversion model-based feedforward from
+ * given model dynamics.
+ *
+ * <p>If given the vector valued function as f(x, u) where x is the state
+ * vector and u is the input vector, the B matrix(continuous input matrix)
+ * is calculated through a {@link edu.wpi.first.wpilibj.system.NumericalJacobian}.
+ * In this case f has to be control-affine (of the form f(x) + Bu).
+ *
+ * <p>The feedforward is calculated as
+ * <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
+ * <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * <p>This feedforward does not account for a dynamic B matrix, B is either
+ * determined or supplied when the feedforward is created and remains constant.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
+public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
+  /**
+   * The current reference state.
+   */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_r;
+
+  /**
+   * The computed feedforward.
+   */
+  private Matrix<Inputs, N1> m_uff;
+
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Inputs> m_B;
+
+  private final Nat<Inputs> m_inputs;
+
+  private final double m_dt;
+
+  /**
+   * The model dynamics.
+   */
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function
+   * of state and input.
+   *
+   * @param states    A {@link Nat} representing the number of states.
+   * @param inputs    A {@link Nat} representing the number of inputs.
+   * @param f         A vector-valued function of x, the state, and
+   *                  u, the input, that returns the derivative of
+   *                  the state vector. HAS to be control-affine
+   *                  (of the form f(x) + Bu).
+   * @param dtSeconds The timestep between calls of calculate().
+   */
+  public ControlAffinePlantInversionFeedforward(
+        Nat<States> states,
+        Nat<Inputs> inputs,
+        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+        double dtSeconds) {
+    this.m_dt = dtSeconds;
+    this.m_f = f;
+    this.m_inputs = inputs;
+
+    this.m_B = NumericalJacobian.numericalJacobianU(states, inputs,
+            m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_uff = new Matrix<>(inputs, Nat.N1());
+
+    reset(m_r);
+  }
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function of state,
+   * and the plant's B(continuous input matrix) matrix.
+   *
+   * @param states    A {@link Nat} representing the number of states.
+   * @param inputs    A {@link Nat} representing the number of inputs.
+   * @param f         A vector-valued function of x, the state,
+   *                  that returns the derivative of the state vector.
+   * @param B         Continuous input matrix of the plant being controlled.
+   * @param dtSeconds The timestep between calls of calculate().
+   */
+  public ControlAffinePlantInversionFeedforward(
+        Nat<States> states,
+        Nat<Inputs> inputs,
+        Function<Matrix<States, N1>, Matrix<States, N1>> f,
+        Matrix<States, Inputs> B,
+        double dtSeconds) {
+    this.m_dt = dtSeconds;
+    this.m_inputs = inputs;
+
+    this.m_f = (x, u) -> f.apply(x);
+    this.m_B = B;
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_uff = new Matrix<>(inputs, Nat.N1());
+
+    reset(m_r);
+  }
+
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> getUff() {
+    return m_uff;
+  }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   *
+   * @return The row of the calculated feedforward.
+   */
+  public double getUff(int row) {
+    return m_uff.get(row, 0);
+  }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the current reference vector r.
+   *
+   * @param row Row of r.
+   *
+   * @return The row of the current reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Resets the feedforward with a zero initial state vector.
+   */
+  public void reset() {
+    m_r.fill(0.0);
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Calculate the feedforward with only the desired
+   * future reference. This uses the internally stored "current"
+   * reference.
+   *
+   * <p>If this method is used the initial state of the system is the one
+   * set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
+   * If the initial state is not set it defaults to a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
+    return calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r     The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    var rDot = (nextR.minus(r)).div(m_dt);
+
+    m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
+
+    m_r = nextR;
+    return m_uff;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
new file mode 100644
index 0000000..0b52c14
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
+ * acting against the force of gravity).
+ */
+@SuppressWarnings("MemberName")
+public class ElevatorFeedforward {
+  public final double ks;
+  public final double kg;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains.  Units of the gain values
+   * will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+    this.ks = ks;
+    this.kg = kg;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains.  Acceleration gain is
+   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv) {
+    this(ks, kg, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+   * be zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - kg - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
+  }
+
+  /**
+   * 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java
new file mode 100644
index 0000000..a4a00ee
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java
@@ -0,0 +1,170 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
+ *
+ * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
+ * where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
+public class LinearPlantInversionFeedforward<States extends Num, Inputs extends Num,
+        Outputs extends Num> {
+  /**
+   * The current reference state.
+   */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_r;
+
+  /**
+   * The computed feedforward.
+   */
+  private Matrix<Inputs, N1> m_uff;
+
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Inputs> m_B;
+
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, States> m_A;
+
+  /**
+   * Constructs a feedforward with the given plant.
+   *
+   * @param plant     The plant being controlled.
+   * @param dtSeconds Discretization timestep.
+   */
+  public LinearPlantInversionFeedforward(
+          LinearSystem<States, Inputs, Outputs> plant,
+          double dtSeconds
+  ) {
+    this(plant.getA(), plant.getB(), dtSeconds);
+  }
+
+  /**
+   * Constructs a feedforward with the given coefficients.
+   *
+   * @param A         Continuous system matrix of the plant being controlled.
+   * @param B         Continuous input matrix of the plant being controlled.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearPlantInversionFeedforward(Matrix<States, States> A, Matrix<States, Inputs> B,
+                                         double dtSeconds) {
+    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+    this.m_A = discABPair.getFirst();
+    this.m_B = discABPair.getSecond();
+
+    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
+    m_uff = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    reset(m_r);
+  }
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> getUff() {
+    return m_uff;
+  }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   *
+   * @return The row of the calculated feedforward.
+   */
+  public double getUff(int row) {
+    return m_uff.get(row, 0);
+  }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the current reference vector r.
+   *
+   * @param row Row of r.
+   *
+   * @return The row of the current reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Resets the feedforward with a zero initial state vector.
+   */
+  public void reset() {
+    m_r.fill(0.0);
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Calculate the feedforward with only the desired
+   * future reference. This uses the internally stored "current"
+   * reference.
+   *
+   * <p>If this method is used the initial state of the system is the one
+   * set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
+   * If the initial state is not set it defaults to a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
+    return calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r     The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
+
+    m_r = nextR;
+    return m_uff;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java
new file mode 100644
index 0000000..195ba4f
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java
@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.math.Drake;
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.Vector;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+
+/**
+ * Contains the controller coefficients and logic for a linear-quadratic
+ * regulator (LQR).
+ * LQRs use the control law u = K(r - x).
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
+      Outputs extends Num> {
+  /**
+   * The current reference state.
+   */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_r;
+
+  /**
+   * The computed and capped controller output.
+   */
+  @SuppressWarnings("MemberName")
+  private Matrix<Inputs, N1> m_u;
+
+  // Controller gain.
+  @SuppressWarnings("MemberName")
+  private Matrix<Inputs, States> m_K;
+
+  /**
+   * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
+   *
+   * @param plant     The plant being controlled.
+   * @param qelms     The maximum desired error tolerance for each state.
+   * @param relms     The maximum desired control effort for each input.
+   * @param dtSeconds Discretization timestep.
+   */
+  public LinearQuadraticRegulator(
+        LinearSystem<States, Inputs, Outputs> plant,
+        Vector<States> qelms,
+        Vector<Inputs> relms,
+        double dtSeconds
+  ) {
+    this(plant.getA(), plant.getB(), StateSpaceUtil.makeCostMatrix(qelms),
+        StateSpaceUtil.makeCostMatrix(relms), dtSeconds);
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A         Continuous system matrix of the plant being controlled.
+   * @param B         Continuous input matrix of the plant being controlled.
+   * @param qelms     The maximum desired error tolerance for each state.
+   * @param relms     The maximum desired control effort for each input.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
+                                  Vector<States> qelms, Vector<Inputs> relms,
+                                  double dtSeconds
+  ) {
+    this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms),
+        dtSeconds);
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   * @param A         Continuous system matrix of the plant being controlled.
+   * @param B         Continuous input matrix of the plant being controlled.
+   * @param Q         The state cost matrix.
+   * @param R         The input cost matrix.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
+                                  Matrix<States, States> Q, Matrix<Inputs, Inputs> R,
+                                  double dtSeconds
+  ) {
+    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+
+    var temp = discB.transpose().times(S).times(discB).plus(R);
+
+    m_K = temp.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));
+
+    reset();
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param states The number of states.
+   * @param inputs The number of inputs.
+   * @param k The gain matrix.
+   */
+  @SuppressWarnings("ParameterName")
+  public LinearQuadraticRegulator(
+      Nat<States> states, Nat<Inputs> inputs,
+      Matrix<Inputs, States> k
+  ) {
+    m_K = k;
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_u = new Matrix<>(inputs, Nat.N1());
+
+    reset();
+  }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return m_u;
+  }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param row Row of u.
+   *
+   * @return The row of the control input vector.
+   */
+  public double getU(int row) {
+    return m_u.get(row, 0);
+  }
+
+  /**
+   * Returns the reference vector r.
+   *
+   * @return The reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param row Row of r.
+   *
+   * @return The row of the reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Returns the controller matrix K.
+   *
+   * @return the controller matrix K.
+   */
+  public Matrix<Inputs, States> getK() {
+    return m_K;
+  }
+
+  /**
+   * Resets the controller.
+   */
+  public void reset() {
+    m_r.fill(0.0);
+    m_u.fill(0.0);
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
+    m_u = m_K.times(m_r.minus(x));
+    return m_u;
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x     The current state x.
+   * @param nextR the next reference vector r.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
+    m_r = nextR;
+    return calculate(x);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
new file mode 100644
index 0000000..ec53d46
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
+ */
+@SuppressWarnings("MemberName")
+public class SimpleMotorFeedforward {
+  public final double ks;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains.  Units of the gain values
+   * will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv, double ka) {
+    this.ks = ks;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains.  Acceleration gain is
+   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv) {
+    this(ks, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
+   * be zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - acceleration * ka) / kv;
+  }
+
+  /**
+   * Calculates the maximum 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
+  }
+
+  /**
+   * Calculates the maximum 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
new file mode 100644
index 0000000..7474b02
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
@@ -0,0 +1,288 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.function.BiFunction;
+
+import edu.wpi.first.math.Drake;
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.NumericalJacobian;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * Kalman filters combine predictions from a model and measurements to give an estimate of the true
+ * system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>The Extended Kalman filter is just like the {@link KalmanFilter Kalman filter}, but we make a
+ * linear approximation of nonlinear dynamics and/or nonlinear measurement models. This means that
+ * the EKF works with nonlinear systems.
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
+      implements KalmanTypeFilter<States, Inputs, Outputs> {
+  private final Nat<States> m_states;
+  private final Nat<Outputs> m_outputs;
+
+  @SuppressWarnings("MemberName")
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+  @SuppressWarnings("MemberName")
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
+  private final Matrix<States, States> m_contQ;
+  private final Matrix<States, States> m_initP;
+  private final Matrix<Outputs, Outputs> m_contR;
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_xHat;
+  @SuppressWarnings("MemberName")
+  private Matrix<States, States> m_P;
+  private double m_dtSeconds;
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * @param states             a Nat representing the number of states.
+   * @param inputs             a Nat representing the number of inputs.
+   * @param outputs            a Nat representing the number of outputs.
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds          Nominal discretization timestep.
+   */
+  @SuppressWarnings("ParameterName")
+  public ExtendedKalmanFilter(
+        Nat<States> states,
+        Nat<Inputs> inputs,
+        Nat<Outputs> outputs,
+        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+        Matrix<States, N1> stateStdDevs,
+        Matrix<Outputs, N1> measurementStdDevs,
+        double dtSeconds
+  ) {
+    m_states = states;
+    m_outputs = outputs;
+
+    m_f = f;
+    m_h = h;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_dtSeconds = dtSeconds;
+
+    reset();
+
+    final var contA = NumericalJacobian
+          .numericalJacobianX(states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
+    final var C = NumericalJacobian
+          .numericalJacobianX(outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
+
+    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    final var discR = Discretization.discretizeR(m_contR, dtSeconds);
+
+    // IsStabilizable(A^T, C^T) will tell us if the system is observable.
+    boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
+    if (isObservable && outputs.getNum() <= states.getNum()) {
+      m_initP = Drake.discreteAlgebraicRiccatiEquation(
+            discA.transpose(), C.transpose(), discQ, discR) ;
+    } else {
+      m_initP = new Matrix<>(states, states);
+    }
+
+    m_P = m_initP;
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_P;
+  }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param row Row of P.
+   * @param col Column of P.
+   * @return the value of the error covariance matrix P at (i, j).
+   */
+  @Override
+  public double getP(int row, int col) {
+    return m_P.get(row, col);
+  }
+
+  /**
+   * Sets the entire error covariance matrix P.
+   *
+   * @param newP The new value of P to use.
+   */
+  @Override
+  public void setP(Matrix<States, States> newP) {
+    m_P = newP;
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return the state estimate x-hat.
+   */
+  @Override
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the value of the state estimate x-hat at i.
+   */
+  @Override
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void setXhat(Matrix<States, N1> xHat) {
+    m_xHat = xHat;
+  }
+
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row   Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  @Override
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  @Override
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_P = m_initP;
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u         New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    predict(u, m_f, dtSeconds);
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u         New control input from controller.
+   * @param f         The function used to linearlize the model.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  public void predict(
+      Matrix<Inputs, N1> u, BiFunction<Matrix<States, N1>,
+        Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      double dtSeconds
+  ) {
+    // Find continuous A
+    final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
+
+    // Find discrete A and Q
+    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    m_xHat = RungeKutta.rungeKutta(f, m_xHat, u, dtSeconds);
+    m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
+    m_dtSeconds = dtSeconds;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    correct(m_outputs, u, y, m_h, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one is
+   * not provided (the two-argument version of this function).
+   *
+   * @param <Rows>  Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u    Same control input used in the predict step.
+   * @param y    Measurement vector.
+   * @param h    A vector-valued function of x and u that returns the measurement
+   *             vector.
+   * @param R    Discrete measurement noise covariance matrix.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public <Rows extends Num> void correct(
+        Nat<Rows> rows, Matrix<Inputs, N1> u,
+        Matrix<Rows, N1> y,
+        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+        Matrix<Rows, Rows> R
+  ) {
+    final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
+
+    final var S = C.times(m_P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PC^T S^-1
+    // KS = PC^T
+    // (KS)^T = (PC^T)^T
+    // S^T K^T = CP^T
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // K^T = S^T.solve(CP^T)
+    // K = (S^T.solve(CP^T))^T
+    //
+    // Now we have the Kalman gain
+    final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
+
+    m_xHat = m_xHat.plus(K.times(y.minus(h.apply(m_xHat, u))));
+    m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java
new file mode 100644
index 0000000..99fa2b7
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java
@@ -0,0 +1,204 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
+ * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
+ * of squares error in the state estimate. This K gain is used to correct the state estimate by
+ * some amount of the difference between the actual measurements and the measurements predicted by
+ * the model.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
+ * theory".
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class KalmanFilter<States extends Num, Inputs extends Num,
+      Outputs extends Num> {
+  private final Nat<States> m_states;
+
+  private final LinearSystem<States, Inputs, Outputs> m_plant;
+
+  /**
+   * The steady-state Kalman gain matrix.
+   */
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Outputs> m_K;
+
+  /**
+   * The state estimate.
+   */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_xHat;
+
+  /**
+   * Constructs a state-space observer with the given plant.
+   *
+   * @param states             A Nat representing the states of the system.
+   * @param outputs            A Nat representing the outputs of the system.
+   * @param plant              The plant used for the prediction step.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds          Nominal discretization timestep.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public KalmanFilter(
+      Nat<States> states, Nat<Outputs> outputs,
+      LinearSystem<States, Inputs, Outputs> plant,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dtSeconds
+  ) {
+    this.m_states = states;
+
+    this.m_plant = plant;
+
+    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
+    var discA = pair.getFirst();
+    var discQ = pair.getSecond();
+
+    var discR = Discretization.discretizeR(contR, dtSeconds);
+
+    var C = plant.getC();
+
+    // isStabilizable(A^T, C^T) will tell us if the system is observable.
+    var isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
+    if (!isObservable) {
+      MathSharedStore.reportError("The system passed to the Kalman filter is not observable!",
+          Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(
+        "The system passed to the Kalman filter is not observable!");
+    }
+
+    var P = new Matrix<>(Drake.discreteAlgebraicRiccatiEquation(
+          discA.transpose(), C.transpose(), discQ, discR));
+
+    var S = C.times(P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PC^T S^-1
+    // KS = PC^T
+    // (KS)^T = (PC^T)^T
+    // S^T K^T = CP^T
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // K^T = S^T.solve(CP^T)
+    // K = (S^T.solve(CP^T))^T
+    m_K = new Matrix<>(S.transpose().getStorage()
+          .solve((C.times(P.transpose())).getStorage()).transpose());
+
+    reset();
+  }
+
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+  }
+
+  /**
+   * Returns the steady-state Kalman gain matrix K.
+   *
+   * @return The steady-state Kalman gain matrix K.
+   */
+  public Matrix<States, Outputs> getK() {
+    return m_K;
+  }
+
+  /**
+   * Returns an element of the steady-state Kalman gain matrix K.
+   *
+   * @param row Row of K.
+   * @param col Column of K.
+   * @return the element (i, j) of the steady-state Kalman gain matrix K.
+   */
+  public double getK(int row, int col) {
+    return m_K.get(row, col);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xhat The state estimate x-hat.
+   */
+  public void setXhat(Matrix<States, N1> xhat) {
+    this.m_xHat = xhat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row   Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return The state estimate x-hat.
+   */
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the state estimate x-hat at i.
+   */
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u         New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the last predict step.
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    final var C = m_plant.getC();
+    final var D = m_plant.getD();
+    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java
new file mode 100644
index 0000000..aa93b29
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+@SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"})
+interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+  Matrix<States, States> getP();
+
+  double getP(int i, int j);
+
+  void setP(Matrix<States, States> newP);
+
+  Matrix<States, N1> getXhat();
+
+  double getXhat(int i);
+
+  void setXhat(Matrix<States, N1> xHat);
+
+  void setXhat(int i, double value);
+
+  void reset();
+
+  void predict(Matrix<Inputs, N1> u, double dtSeconds);
+
+  void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
new file mode 100644
index 0000000..56e9288
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
@@ -0,0 +1,168 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * Generates sigma points and weights according to Van der Merwe's 2004
+ * dissertation[1] for the UnscentedKalmanFilter class.
+ *
+ * <p>It parametrizes the sigma points using alpha, beta, kappa terms, and is the
+ * version seen in most publications. Unless you know better, this should be
+ * your default choice.
+ *
+ * <p>States is the dimensionality of the state. 2*States+1 weights will be
+ * generated.
+ *
+ * <p>[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
+ * Inference in Dynamic State-Space Models" (Doctoral dissertation)
+ */
+public class MerweScaledSigmaPoints<S extends Num> {
+
+  private final double m_alpha;
+  private final int m_kappa;
+  private final Nat<S> m_states;
+  private Matrix<?, N1> m_wm;
+  private Matrix<?, N1> m_wc;
+
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points.
+   *
+   * @param states an instance of Num that represents the number of states.
+   * @param alpha  Determines the spread of the sigma points around the mean.
+   *               Usually a small positive value (1e-3).
+   * @param beta   Incorporates prior knowledge of the distribution of the mean.
+   *               For Gaussian distributions, beta = 2 is optimal.
+   * @param kappa  Secondary scaling parameter usually set to 0 or 3 - States.
+   */
+  public MerweScaledSigmaPoints(Nat<S> states, double alpha, double beta, int kappa) {
+    this.m_states = states;
+    this.m_alpha = alpha;
+    this.m_kappa = kappa;
+
+    computeWeights(beta);
+  }
+
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points with default values for alpha,
+   * beta, and kappa.
+   *
+   * @param states an instance of Num that represents the number of states.
+   */
+  public MerweScaledSigmaPoints(Nat<S> states) {
+    this(states, 1e-3, 2, 3 - states.getNum());
+  }
+
+  /**
+   * Returns number of sigma points for each variable in the state x.
+   *
+   * @return The number of sigma points for each variable in the state x.
+   */
+  public int getNumSigmas() {
+    return 2 * m_states.getNum() + 1;
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean
+   * (x) and covariance(P) of the filter.
+   *
+   * @param x An array of the means.
+   * @param P Covariance of the filter.
+   * @return Two dimensional array of sigma points. Each column contains all of
+   *         the sigmas for one dimension in the problem space. Ordered by
+   *         Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public Matrix<S, ?> sigmaPoints(
+          Matrix<S, N1> x,
+          Matrix<S, S> P) {
+    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+
+    var intermediate = P.times(lambda + m_states.getNum());
+    var U = intermediate.lltDecompose(true); // Lower triangular
+
+    // 2 * states + 1 by states
+    Matrix<S, ?> sigmas = new Matrix<>(
+        new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+    sigmas.setColumn(0, x);
+    for (int k = 0; k < m_states.getNum(); k++) {
+      var xPlusU = x.plus(U.extractColumnVector(k));
+      var xMinusU = x.minus(U.extractColumnVector(k));
+      sigmas.setColumn(k + 1, xPlusU);
+      sigmas.setColumn(m_states.getNum() + k + 1, xMinusU);
+    }
+
+    return new Matrix<>(sigmas);
+  }
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   */
+  @SuppressWarnings("LocalVariableName")
+  private void computeWeights(double beta) {
+    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+    double c = 0.5 / (m_states.getNum() + lambda);
+
+    Matrix<?, N1> wM = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
+    Matrix<?, N1> wC = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
+    wM.fill(c);
+    wC.fill(c);
+
+    wM.set(0, 0, lambda / (m_states.getNum() + lambda));
+    wC.set(0, 0, lambda / (m_states.getNum() + lambda) + (1 - Math.pow(m_alpha, 2) + beta));
+
+    this.m_wm = wM;
+    this.m_wc = wC;
+  }
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   *
+   * @return the weight for each sigma point for the mean.
+   */
+  public Matrix<?, N1> getWm() {
+    return m_wm;
+  }
+
+  /**
+   * Returns an element of the weight for each sigma point for the mean.
+   *
+   * @param element Element of vector to return.
+   * @return the element i's weight for the mean.
+   */
+  public double getWm(int element) {
+    return m_wm.get(element, 0);
+  }
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   *
+   * @return the weight for each sigma point for the covariance.
+   */
+  public Matrix<?, N1> getWc() {
+    return m_wc;
+  }
+
+  /**
+   * Returns an element of the weight for each sigma point for the covariance.
+   *
+   * @param element Element of vector to return.
+   * @return The element I's weight for the covariance.
+   */
+  public double getWc(int element) {
+    return m_wc.get(element, 0);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
new file mode 100644
index 0000000..ca99153
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
@@ -0,0 +1,316 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.function.BiFunction;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.NumericalJacobian;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.Pair;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true ystem state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>The Unscented Kalman filter is similar to the {@link KalmanFilter Kalman filter}, except that
+ * it propagates carefully chosen points called sigma points through the non-linear model to obtain
+ * an estimate of the true covariance (as opposed to a linearized version of it). This means that
+ * the UKF works with nonlinear systems.
+ */
+@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
+public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
+      Outputs extends Num> implements KalmanTypeFilter<States, Inputs, Outputs> {
+
+  private final Nat<States> m_states;
+  private final Nat<Outputs> m_outputs;
+
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
+
+  private Matrix<States, N1> m_xHat;
+  private Matrix<States, States> m_P;
+  private final Matrix<States, States> m_contQ;
+  private final Matrix<Outputs, Outputs> m_contR;
+  private Matrix<States, ?> m_sigmasF;
+  private double m_dtSeconds;
+
+  private final MerweScaledSigmaPoints<States> m_pts;
+
+  /**
+   * Constructs an Unscented Kalman Filter.
+   *
+   * @param states             A Nat representing the number of states.
+   * @param outputs            A Nat representing the number of outputs.
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds          Nominal discretization timestep.
+   */
+  @SuppressWarnings("ParameterName")
+  public UnscentedKalmanFilter(Nat<States> states, Nat<Outputs> outputs,
+                               BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>,
+                                   Matrix<States, N1>> f,
+                               BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>,
+                                   Matrix<Outputs, N1>> h,
+                               Matrix<States, N1> stateStdDevs,
+                               Matrix<Outputs, N1> measurementStdDevs,
+                               double dtSeconds) {
+    this.m_states = states;
+    this.m_outputs = outputs;
+
+    m_f = f;
+    m_h = h;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    m_dtSeconds = dtSeconds;
+
+    m_pts = new MerweScaledSigmaPoints<>(states);
+
+    reset();
+  }
+
+  @SuppressWarnings({"ParameterName", "LocalVariableName", "PMD.CyclomaticComplexity"})
+  static <S extends Num, C extends Num>
+       Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
+        Nat<S> s, Nat<C> dim, Matrix<C, ?> sigmas, Matrix<?, N1> Wm, Matrix<?, N1> Wc
+  ) {
+    if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
+      throw new IllegalArgumentException("Sigmas must be covDim by 2 * states + 1! Got "
+            + sigmas.getNumRows() + " by " + sigmas.getNumCols());
+    }
+
+    if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1 ) {
+      throw new IllegalArgumentException("Wm must be 2 * states + 1 by 1! Got "
+            + Wm.getNumRows() + " by " + Wm.getNumCols());
+    }
+
+    if (Wc.getNumRows() != 2 * s.getNum() + 1 || Wc.getNumCols() != 1) {
+      throw new IllegalArgumentException("Wc must be 2 * states + 1 by 1! Got "
+            + Wc.getNumRows() + " by " + Wc.getNumCols());
+    }
+
+    // New mean is just the sum of the sigmas * weight
+    // dot = \Sigma^n_1 (W[k]*Xi[k])
+    Matrix<C, N1> x = sigmas.times(Matrix.changeBoundsUnchecked(Wm));
+
+    // New covariance is the sum of the outer product of the residuals times the
+    // weights
+    Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
+    for (int i = 0; i < 2 * s.getNum() + 1; i++) {
+      y.setColumn(i, sigmas.extractColumnVector(i).minus(x));
+    }
+    Matrix<C, C> P = y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
+          .times(Matrix.changeBoundsUnchecked(y.transpose()));
+
+    return new Pair<>(x, P);
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_P;
+  }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param row Row of P.
+   * @param col Column of P.
+   * @return the value of the error covariance matrix P at (i, j).
+   */
+  @Override
+  public double getP(int row, int col) {
+    return m_P.get(row, col);
+  }
+
+  /**
+   * Sets the entire error covariance matrix P.
+   *
+   * @param newP The new value of P to use.
+   */
+  @Override
+  public void setP(Matrix<States, States> newP) {
+    m_P = newP;
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return the state estimate x-hat.
+   */
+  @Override
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the value of the state estimate x-hat at i.
+   */
+  @Override
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void setXhat(Matrix<States, N1> xHat) {
+    m_xHat = xHat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row   Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  @Override
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  /**
+   * Resets the observer.
+   */
+  @Override
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_P = new Matrix<>(m_states, m_states);
+    m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u         New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    // Discretize Q before projecting mean and covariance forward
+    Matrix<States, States> contA =
+        NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
+    var discQ =
+        Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
+
+    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+
+    for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
+      Matrix<States, N1> x = sigmas.extractColumnVector(i);
+
+      m_sigmasF.setColumn(i, RungeKutta.rungeKutta(m_f, x, u, dtSeconds));
+    }
+
+    var ret = unscentedTransform(m_states, m_states,
+          m_sigmasF, m_pts.getWm(), m_pts.getWc());
+
+    m_xHat = ret.getFirst();
+    m_P = ret.getSecond().plus(discQ);
+    m_dtSeconds = dtSeconds;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    correct(m_outputs, u, y, m_h, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns
+   *          the measurement vector.
+   * @param R Measurement noise covariance matrix.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public <R extends Num> void correct(
+        Nat<R> rows, Matrix<Inputs, N1> u,
+        Matrix<R, N1> y,
+        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
+        Matrix<R, R> R) {
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
+
+    // Transform sigma points into measurement space
+    Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(
+          rows.getNum(), 2 * m_states.getNum() + 1));
+    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
+      Matrix<R, N1> hRet = h.apply(
+            sigmas.extractColumnVector(i),
+            u
+      );
+      sigmasH.setColumn(i, hRet);
+    }
+
+    // Mean and covariance of prediction passed through unscented transform
+    var transRet = unscentedTransform(m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc());
+    var yHat = transRet.getFirst();
+    var Py = transRet.getSecond().plus(discR);
+
+    // Compute cross covariance of the state and the measurements
+    Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
+    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
+      var temp =
+            m_sigmasF.extractColumnVector(i).minus(m_xHat)
+                    .times(sigmasH.extractColumnVector(i).minus(yHat).transpose());
+
+      Pxy = Pxy.plus(temp.times(m_pts.getWc(i)));
+    }
+
+    // K = P_{xy} Py^-1
+    // K^T = P_y^T^-1 P_{xy}^T
+    // P_y^T K^T = P_{xy}^T
+    // K^T = P_y^T.solve(P_{xy}^T)
+    // K = (P_y^T.solve(P_{xy}^T)^T
+    Matrix<States, R> K = new Matrix<>(
+          Py.transpose().solve(Pxy.transpose()).transpose()
+    );
+
+    m_xHat = m_xHat.plus(K.times(y.minus(yHat)));
+    m_P = m_P.minus(K.times(Py).times(K.transpose()));
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
new file mode 100644
index 0000000..a0e3b9a
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
@@ -0,0 +1,252 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Pose2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   * (Translation2d{0, 0} and Rotation{0})
+   */
+  public Pose2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation    The rotational component of the pose.
+   */
+  @JsonCreator
+  public Pose2d(@JsonProperty(required = true, value = "translation") Translation2d translation,
+                @JsonProperty(required = true, value = "rotation") Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Convenience constructors that takes in x and y values directly instead of
+   * having to construct a Translation2d.
+   *
+   * @param x        The x component of the translational component of the pose.
+   * @param y        The y component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  @SuppressWarnings("ParameterName")
+  public Pose2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * <p>The matrix multiplication is as follows
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [0,    0,   1][transform.t]
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d plus(Transform2d other) {
+    return transformBy(other);
+  }
+
+  /**
+   * Returns the Transform2d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  public Transform2d minus(Pose2d other) {
+    final var pose = this.relativeTo(other);
+    return new Transform2d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the pose.
+   */
+  @JsonProperty
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  @JsonProperty
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose.
+   * See + operator for the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d transformBy(Transform2d other) {
+    return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+        m_rotation.plus(other.getRotation()));
+  }
+
+  /**
+   * Returns the other pose relative to the current pose.
+   *
+   * <p>This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   *              the current pose will be converted into.
+   * @return The current pose relative to the new origin pose.
+   */
+  public Pose2d relativeTo(Pose2d other) {
+    var transform = new Transform2d(other, this);
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Obtain a new Pose2d from a (constant curvature) velocity.
+   *
+   * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">
+   * Controls Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for
+   * a derivation.
+   *
+   * <p>The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs exp() on the previous known
+   * field-relative pose with the argument being the twist, the user will
+   * receive the new field-relative pose.
+   *
+   * <p>"Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @param twist The change in pose in the robot's coordinate frame since the
+   *              previous pose update. For example, if a non-holonomic robot moves forward
+   *              0.01 meters and changes angle by 0.5 degrees since the previous pose update,
+   *              the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   * @return The new pose of the robot.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public Pose2d exp(Twist2d twist) {
+    double dx = twist.dx;
+    double dy = twist.dy;
+    double dtheta = twist.dtheta;
+
+    double sinTheta = Math.sin(dtheta);
+    double cosTheta = Math.cos(dtheta);
+
+    double s;
+    double c;
+    if (Math.abs(dtheta) < 1E-9) {
+      s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+      c = 0.5 * dtheta;
+    } else {
+      s = sinTheta / dtheta;
+      c = (1 - cosTheta) / dtheta;
+    }
+    var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
+        new Rotation2d(cosTheta, sinTheta));
+
+    return this.plus(transform);
+  }
+
+  /**
+   * Returns a Twist2d that maps this pose to the end pose. If c is the output
+   * of a.Log(b), then a.Exp(c) would yield b.
+   *
+   * @param end The end pose for the transformation.
+   * @return The twist that maps this to end.
+   */
+  public Twist2d log(Pose2d end) {
+    final var transform = end.relativeTo(this);
+    final var dtheta = transform.getRotation().getRadians();
+    final var halfDtheta = dtheta / 2.0;
+
+    final var cosMinusOne = transform.getRotation().getCos() - 1;
+
+    double halfThetaByTanOfHalfDtheta;
+    if (Math.abs(cosMinusOne) < 1E-9) {
+      halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+    } else {
+      halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
+    }
+
+    Translation2d translationPart = transform.getTranslation().rotateBy(
+        new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
+    ).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
+
+    return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Pose2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Pose2d) {
+      return ((Pose2d) obj).m_translation.equals(m_translation)
+          && ((Pose2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
new file mode 100644
index 0000000..e1c25eb
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
@@ -0,0 +1,215 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+/**
+ * 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 {
+  private final double m_value;
+  private final double m_cos;
+  private final double m_sin;
+
+  /**
+   * Constructs a Rotation2d with a default angle of 0 degrees.
+   */
+  public Rotation2d() {
+    m_value = 0.0;
+    m_cos = 1.0;
+    m_sin = 0.0;
+  }
+
+  /**
+   * Constructs a Rotation2d with the given radian value.
+   * The x and y don't have to be normalized.
+   *
+   * @param value The value of the angle in radians.
+   */
+  @JsonCreator
+  public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
+    m_value = value;
+    m_cos = Math.cos(value);
+    m_sin = Math.sin(value);
+  }
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine)
+   * components.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  @SuppressWarnings("ParameterName")
+  public Rotation2d(double x, double y) {
+    double magnitude = Math.hypot(x, y);
+    if (magnitude > 1e-6) {
+      m_sin = y / magnitude;
+      m_cos = x / magnitude;
+    } else {
+      m_sin = 0.0;
+      m_cos = 1.0;
+    }
+    m_value = Math.atan2(m_sin, m_cos);
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given degree value.
+   *
+   * @param degrees The value of the angle in degrees.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromDegrees(double degrees) {
+    return new Rotation2d(Math.toRadians(degrees));
+  }
+
+  /**
+   * Adds two rotations together, with the result being bounded between -pi and
+   * pi.
+   *
+   * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
+   * Rotation2d{-pi/2}
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation2d plus(Rotation2d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
+   * Rotation2d{-pi/2}
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  public Rotation2d minus(Rotation2d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of
+   * the current angular value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  public Rotation2d unaryMinus() {
+    return new Rotation2d(-m_value);
+  }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation2d.
+   */
+  public Rotation2d times(double scalar) {
+    return new Rotation2d(m_value * scalar);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * <p>The matrix multiplication is as follows:
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = atan2(cos_new, sin_new)
+   *
+   * @param other The rotation to rotate by.
+   * @return The new rotated Rotation2d.
+   */
+  public Rotation2d rotateBy(Rotation2d other) {
+    return new Rotation2d(
+        m_cos * other.m_cos - m_sin * other.m_sin,
+        m_cos * other.m_sin + m_sin * other.m_cos
+    );
+  }
+
+  /**
+   * Returns the radian value of the rotation.
+   *
+   * @return The radian value of the rotation.
+   */
+  @JsonProperty
+  public double getRadians() {
+    return m_value;
+  }
+
+  /**
+   * Returns the degree value of the rotation.
+   *
+   * @return The degree value of the rotation.
+   */
+  public double getDegrees() {
+    return Math.toDegrees(m_value);
+  }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  public double getCos() {
+    return m_cos;
+  }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  public double getSin() {
+    return m_sin;
+  }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  public double getTan() {
+    return m_sin / m_cos;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+  }
+
+  /**
+   * Checks equality between this Rotation2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Rotation2d) {
+      return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_value);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
new file mode 100644
index 0000000..16746d5
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+public class Transform2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param last    The final pose for the transformation.
+   */
+  public Transform2d(Pose2d initial, Pose2d last) {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    m_translation = last.getTranslation().minus(initial.getTranslation())
+            .rotateBy(initial.getRotation().unaryMinus());
+
+    m_rotation = last.getRotation().minus(initial.getRotation());
+  }
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation    Rotational component of the transform.
+   */
+  public Transform2d(Translation2d translation, Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  public Transform2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  public Transform2d times(double scalar) {
+    return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the transform.
+   */
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  public Transform2d inverse() {
+    // We are rotating the difference between the translations
+    // using a clockwise rotation matrix. This transforms the global
+    // delta into a local delta (relative to the initial pose).
+    return new Transform2d(getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
+        getRotation().unaryMinus());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Transform2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Transform2d) {
+      return ((Transform2d) obj).m_translation.equals(m_translation)
+          && ((Transform2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
new file mode 100644
index 0000000..5365759
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
@@ -0,0 +1,215 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * <p>This assumes that you are using conventional mathematical axes.
+ * When the robot is placed on the origin, facing toward the X direction,
+ * moving forward increases the X, whereas moving to the left increases the Y.
+ */
+@SuppressWarnings({"ParameterName", "MemberName"})
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Translation2d {
+  private final double m_x;
+  private final double m_y;
+
+  /**
+   * Constructs a Translation2d with X and Y components equal to zero.
+   */
+  public Translation2d() {
+    this(0.0, 0.0);
+  }
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  @JsonCreator
+  public Translation2d(@JsonProperty(required = true, value = "x") double x,
+                       @JsonProperty(required = true, value = "y") double y) {
+    m_x = x;
+    m_y = y;
+  }
+
+  /**
+   * Constructs a Translation2d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle    The angle between the x-axis and the translation vector.
+   */
+  public Translation2d(double distance, Rotation2d angle) {
+    m_x = distance * angle.getCos();
+    m_y = distance * angle.getSin();
+  }
+
+  /**
+   * Calculates the distance between two translations in 2d space.
+   *
+   * <p>This function uses the pythagorean theorem to calculate the distance.
+   * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation2d other) {
+    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  @JsonProperty
+  public double getX() {
+    return m_x;
+  }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  @JsonProperty
+  public double getY() {
+    return m_y;
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  public double getNorm() {
+    return Math.hypot(m_x, m_y);
+  }
+
+  /**
+   * Applies a rotation to the translation in 2d space.
+   *
+   * <p>This multiplies the translation vector by a counterclockwise rotation
+   * matrix of the given angle.
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   *
+   * <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+   * Translation2d of {0, 2}.
+   *
+   * @param other The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation2d rotateBy(Rotation2d other) {
+    return new Translation2d(
+            m_x * other.getCos() - m_y * other.getSin(),
+            m_x * other.getSin() + m_y * other.getCos()
+    );
+  }
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to
+   * vector addition.
+   *
+   * <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+   * Translation2d{3.0, 8.0}
+   *
+   * @param other The translation to add.
+   * @return The sum of the translations.
+   */
+  public Translation2d plus(Translation2d other) {
+    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+  }
+
+  /**
+   * Subtracts the other translation from the other translation and returns the
+   * difference.
+   *
+   * <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+   * Translation2d{4.0, 2.0}
+   *
+   * @param other The translation to subtract.
+   * @return The difference between the two translations.
+   */
+  public Translation2d minus(Translation2d other) {
+    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
+  }
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * rotating by 180 degrees, flipping the point over both axes, or simply
+   * negating both components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  public Translation2d unaryMinus() {
+    return new Translation2d(-m_x, -m_y);
+  }
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled translation.
+   */
+  public Translation2d times(double scalar) {
+    return new Translation2d(m_x * scalar, m_y * scalar);
+  }
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation2d div(double scalar) {
+    return new Translation2d(m_x / scalar, m_y / scalar);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+  }
+
+  /**
+   * Checks equality between this Translation2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Translation2d) {
+      return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
+          && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_x, m_y);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
new file mode 100644
index 0000000..1482902
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+/**
+ * A change in distance along arc since the last pose update. We can use ideas
+ * from differential calculus to create new Pose2ds from a Twist2d and vise
+ * versa.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+@SuppressWarnings("MemberName")
+public class Twist2d {
+  /**
+   * Linear "dx" component.
+   */
+  public double dx;
+
+  /**
+   * Linear "dy" component.
+   */
+  public double dy;
+
+  /**
+   * Angular "dtheta" component (radians).
+   */
+  public double dtheta;
+
+  public Twist2d() {
+  }
+
+  /**
+   * Constructs a Twist2d with the given values.
+   * @param dx Change in x direction relative to robot.
+   * @param dy Change in y direction relative to robot.
+   * @param dtheta Change in angle relative to robot.
+   */
+  public Twist2d(double dx, double dy, double dtheta) {
+    this.dx = dx;
+    this.dy = dy;
+    this.dtheta = dtheta;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+  }
+
+  /**
+   * Checks equality between this Twist2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Twist2d) {
+      return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
+          && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
+          && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(dx, dy, dtheta);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
new file mode 100644
index 0000000..a6878b3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents the speed of a robot chassis. Although this struct contains
+ * similar members compared to a Twist2d, they do NOT represent the same thing.
+ * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference,
+ * this ChassisSpeeds struct represents a velocity w.r.t to the robot frame of
+ * reference.
+ *
+ * <p>A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have a dy component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+@SuppressWarnings("MemberName")
+public class ChassisSpeeds {
+  /**
+   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+   */
+  public double vxMetersPerSecond;
+
+  /**
+   * Represents sideways velocity w.r.t the robot frame of reference. (Left is +)
+   */
+  public double vyMetersPerSecond;
+
+  /**
+   * Represents the angular velocity of the robot frame. (CCW is +)
+   */
+  public double omegaRadiansPerSecond;
+
+  /**
+   * Constructs a ChassisSpeeds with zeros for dx, dy, and theta.
+   */
+  public ChassisSpeeds() {
+  }
+
+  /**
+   * Constructs a ChassisSpeeds object.
+   *
+   * @param vxMetersPerSecond     Forward velocity.
+   * @param vyMetersPerSecond     Sideways velocity.
+   * @param omegaRadiansPerSecond Angular velocity.
+   */
+  public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond,
+                       double omegaRadiansPerSecond) {
+    this.vxMetersPerSecond = vxMetersPerSecond;
+    this.vyMetersPerSecond = vyMetersPerSecond;
+    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
+  }
+
+  /**
+   * Converts a user provided field-relative set of speeds into a robot-relative
+   * ChassisSpeeds object.
+   *
+   * @param vxMetersPerSecond     The component of speed in the x direction relative to the field.
+   *                              Positive x is away from your alliance wall.
+   * @param vyMetersPerSecond     The component of speed in the y direction relative to the field.
+   *                              Positive y is to your left when standing behind the alliance wall.
+   * @param omegaRadiansPerSecond The angular rate of the robot.
+   * @param robotAngle            The angle of the robot as measured by a gyroscope. The robot's
+   *                              angle is considered to be zero when it is facing directly away
+   *                              from your alliance station wall. Remember that this should
+   *                              be CCW positive.
+   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
+   */
+  public static ChassisSpeeds fromFieldRelativeSpeeds(
+      double vxMetersPerSecond, double vyMetersPerSecond,
+      double omegaRadiansPerSecond, Rotation2d robotAngle) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
+        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
+        omegaRadiansPerSecond
+    );
+  }
+
+  @Override
+  public String toString() {
+    return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
+        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
new file mode 100644
index 0000000..309f531
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to
+ * left and right wheel velocities for a differential drive.
+ *
+ * <p>Inverse kinematics converts a desired chassis speed into left and right
+ * velocity components whereas forward kinematics converts left and right
+ * component velocities into a linear and angular chassis speed.
+ */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveKinematics {
+  public final double trackWidthMeters;
+
+  /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is
+   *                         the distance between the left wheels and right wheels.
+   *                         However, the empirical value may be larger than the physical
+   *                         measured value due to scrubbing effects.
+   */
+  public DifferentialDriveKinematics(double trackWidthMeters) {
+    this.trackWidthMeters = trackWidthMeters;
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
+  }
+
+  /**
+   * Returns a chassis speed from left and right component velocities using
+   * forward kinematics.
+   *
+   * @param wheelSpeeds The left and right velocities.
+   * @return The chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
+    return new ChassisSpeeds(
+        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
+        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
+            / trackWidthMeters
+    );
+  }
+
+  /**
+   * Returns left and right component velocities from a chassis speed using
+   * inverse kinematics.
+   *
+   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
+   *                      represent the chassis' speed.
+   * @return The left and right velocities.
+   */
+  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return new DifferentialDriveWheelSpeeds(
+        chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2
+          * chassisSpeeds.omegaRadiansPerSecond,
+        chassisSpeeds.vxMetersPerSecond + trackWidthMeters / 2
+          * chassisSpeeds.omegaRadiansPerSecond
+    );
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
new file mode 100644
index 0000000..86470f8
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+
+/**
+ * Class for differential drive odometry. Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from
+ * 2 encoders and a gyroscope.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ *
+ * <p>It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
+ */
+public class DifferentialDriveOdometry {
+  private Pose2d m_poseMeters;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private double m_prevLeftDistance;
+  private double m_prevRightDistance;
+
+  /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * @param gyroAngle         The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle,
+                                   Pose2d initialPoseMeters) {
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
+  }
+
+  /**
+   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
+    this(gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+
+    m_prevLeftDistance = 0.0;
+    m_prevRightDistance = 0.0;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+
+  /**
+   * Updates the robot position on the field using distance measurements from encoders. This
+   * method is more numerically accurate than using velocities to integrate the pose and
+   * is also advantageous for teams that are using lower CPR encoders.
+   *
+   * @param gyroAngle           The angle reported by the gyroscope.
+   * @param leftDistanceMeters  The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
+                       double rightDistanceMeters) {
+    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
+    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
+
+    m_prevLeftDistance = leftDistanceMeters;
+    m_prevRightDistance = rightDistanceMeters;
+
+    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var newPose = m_poseMeters.exp(
+        new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
new file mode 100644
index 0000000..1716430
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+/**
+ * Represents the wheel speeds for a differential drive drivetrain.
+ */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveWheelSpeeds {
+  /**
+   * Speed of the left side of the robot.
+   */
+  public double leftMetersPerSecond;
+
+  /**
+   * Speed of the right side of the robot.
+   */
+  public double rightMetersPerSecond;
+
+  /**
+   * Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds.
+   */
+  public DifferentialDriveWheelSpeeds() {
+  }
+
+  /**
+   * Constructs a DifferentialDriveWheelSpeeds.
+   *
+   * @param leftMetersPerSecond  The left speed.
+   * @param rightMetersPerSecond The right speed.
+   */
+  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
+    this.leftMetersPerSecond = leftMetersPerSecond;
+    this.rightMetersPerSecond = rightMetersPerSecond;
+  }
+
+  /**
+   * 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.
+   *
+   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
+        leftMetersPerSecond, rightMetersPerSecond);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
new file mode 100644
index 0000000..8c1d5d3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
@@ -0,0 +1,172 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds.
+ *
+ * <p>The inverse kinematics (converting from a desired chassis velocity to
+ * individual wheel speeds) uses the relative locations of the wheels with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion maneuvers.
+ *
+ * <p>Forward kinematics (converting an array of wheel speeds into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
+ * multiply by [wheelSpeeds] to get our chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+public class MecanumDriveKinematics {
+  private SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final Translation2d m_frontLeftWheelMeters;
+  private final Translation2d m_frontRightWheelMeters;
+  private final Translation2d m_rearLeftWheelMeters;
+  private final Translation2d m_rearRightWheelMeters;
+
+  private Translation2d m_prevCoR = new Translation2d();
+
+  /**
+   * Constructs a mecanum drive kinematics object.
+   *
+   * @param frontLeftWheelMeters  The location of the front-left wheel relative to the
+   *                              physical center of the robot.
+   * @param frontRightWheelMeters The location of the front-right wheel relative to
+   *                              the physical center of the robot.
+   * @param rearLeftWheelMeters   The location of the rear-left wheel relative to the
+   *                              physical center of the robot.
+   * @param rearRightWheelMeters  The location of the rear-right wheel relative to the
+   *                              physical center of the robot.
+   */
+  public MecanumDriveKinematics(Translation2d frontLeftWheelMeters,
+                                Translation2d frontRightWheelMeters,
+                                Translation2d rearLeftWheelMeters,
+                                Translation2d rearRightWheelMeters) {
+    m_frontLeftWheelMeters = frontLeftWheelMeters;
+    m_frontRightWheelMeters = frontRightWheelMeters;
+    m_rearLeftWheelMeters = rearLeftWheelMeters;
+    m_rearRightWheelMeters = rearRightWheelMeters;
+
+    m_inverseKinematics = new SimpleMatrix(4, 3);
+
+    setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
+        rearLeftWheelMeters, rearRightWheelMeters);
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
+  }
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
+   * method is often used to convert joystick values into wheel speeds.
+   *
+   * <p>This function also supports variable centers of rotation. During normal
+   * operations, the center of rotation is usually the same as the physical
+   * center of the robot; therefore, the argument is defaulted to that use case.
+   * However, if you wish to change the center of rotation for evasive
+   * maneuvers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds    The desired chassis speed.
+   * @param centerOfRotationMeters The center of rotation. For example, if you set the
+   *                         center of rotation at one corner of the robot and provide
+   *                         a chassis speed that only has a dtheta 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.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
+                                               Translation2d centerOfRotationMeters) {
+    // We have a new center of rotation. We need to compute the matrix again.
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
+      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
+      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
+      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
+
+      setInverseKinematics(fl, fr, rl, rr);
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(0, 0,
+        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond);
+
+    var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    return new MecanumDriveWheelSpeeds(
+        wheelsMatrix.get(0, 0),
+        wheelsMatrix.get(1, 0),
+        wheelsMatrix.get(2, 0),
+        wheelsMatrix.get(3, 0)
+    );
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
+   * information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return toWheelSpeeds(chassisSpeeds, new Translation2d());
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
+   * This method is often used for odometry -- determining the robot's position on the field using
+   * data from the real-world speed of each wheel on the robot.
+   *
+   * @param wheelSpeeds The current mecanum drive wheel speeds.
+   * @return The resulting chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+    var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
+    wheelSpeedsMatrix.setColumn(0, 0,
+        wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond,
+        wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond
+    );
+    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
+
+    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.get(2, 0));
+  }
+
+  /**
+   * Construct inverse kinematics matrix from wheel locations.
+   *
+   * @param fl The location of the front-left wheel relative to the physical center of the robot.
+   * @param fr The location of the front-right wheel relative to the physical center of the robot.
+   * @param rl The location of the rear-left wheel relative to the physical center of the robot.
+   * @param rr The location of the rear-right wheel relative to the physical center of the robot.
+   */
+  private void setInverseKinematics(Translation2d fl, Translation2d fr,
+                                    Translation2d rl, Translation2d rr) {
+    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
+    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
+    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
+    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
+    m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
new file mode 100644
index 0000000..756bb60
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+/**
+ * Represents the motor voltages for a mecanum drive drivetrain.
+ */
+@SuppressWarnings("MemberName")
+public class MecanumDriveMotorVoltages {
+  /**
+   * Voltage of the front left motor.
+   */
+  public double frontLeftVoltage;
+
+  /**
+   * Voltage of the front right motor.
+   */
+  public double frontRightVoltage;
+
+  /**
+   * Voltage of the rear left motor.
+   */
+  public double rearLeftVoltage;
+
+  /**
+   * Voltage of the rear right motor.
+   */
+  public double rearRightVoltage;
+
+  /**
+   * Constructs a MecanumDriveMotorVoltages with zeros for all member fields.
+   */
+  public MecanumDriveMotorVoltages() {
+  }
+
+  /**
+   * Constructs a MecanumDriveMotorVoltages.
+   *
+   * @param frontLeftVoltage  Voltage of the front left motor.
+   * @param frontRightVoltage Voltage of the front right motor.
+   * @param rearLeftVoltage   Voltage of the rear left motor.
+   * @param rearRightVoltage  Voltage of the rear right motor.
+   */
+  public MecanumDriveMotorVoltages(double frontLeftVoltage,
+                                 double frontRightVoltage,
+                                 double rearLeftVoltage,
+                                 double rearRightVoltage) {
+    this.frontLeftVoltage = frontLeftVoltage;
+    this.frontRightVoltage = frontRightVoltage;
+    this.rearLeftVoltage = rearLeftVoltage;
+    this.rearRightVoltage = rearRightVoltage;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+            + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
+        rearLeftVoltage, rearRightVoltage);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
new file mode 100644
index 0000000..cd84bdf
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
+
+/**
+ * Class for mecanum drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * mecanum wheel encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+public class MecanumDriveOdometry {
+  private final MecanumDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a MecanumDriveOdometry object.
+   *
+   * @param kinematics        The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle         The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
+                              Pose2d initialPoseMeters) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
+  }
+
+  /**
+   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param currentTimeSeconds The current time in seconds.
+   * @param gyroAngle          The angle reported by the gyroscope.
+   * @param wheelSpeeds        The current wheel speeds.
+   * @return The new pose of the robot.
+   */
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
+                               MecanumDriveWheelSpeeds wheelSpeeds) {
+    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
+    var newPose = m_poseMeters.exp(
+        new Twist2d(chassisState.vxMetersPerSecond * period,
+            chassisState.vyMetersPerSecond * period,
+            angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method automatically calculates the
+   * current time to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param gyroAngle   The angle reported by the gyroscope.
+   * @param wheelSpeeds The current wheel speeds.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle,
+                       MecanumDriveWheelSpeeds wheelSpeeds) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle,
+        wheelSpeeds);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
new file mode 100644
index 0000000..f00e409
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import java.util.stream.DoubleStream;
+
+@SuppressWarnings("MemberName")
+public class MecanumDriveWheelSpeeds {
+  /**
+   * Speed of the front left wheel.
+   */
+  public double frontLeftMetersPerSecond;
+
+  /**
+   * Speed of the front right wheel.
+   */
+  public double frontRightMetersPerSecond;
+
+  /**
+   * Speed of the rear left wheel.
+   */
+  public double rearLeftMetersPerSecond;
+
+  /**
+   * Speed of the rear right wheel.
+   */
+  public double rearRightMetersPerSecond;
+
+  /**
+   * Constructs a MecanumDriveWheelSpeeds with zeros for all member fields.
+   */
+  public MecanumDriveWheelSpeeds() {
+  }
+
+  /**
+   * Constructs a MecanumDriveWheelSpeeds.
+   *
+   * @param frontLeftMetersPerSecond  Speed of the front left wheel.
+   * @param frontRightMetersPerSecond Speed of the front right wheel.
+   * @param rearLeftMetersPerSecond   Speed of the rear left wheel.
+   * @param rearRightMetersPerSecond  Speed of the rear right wheel.
+   */
+  public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond,
+                                 double frontRightMetersPerSecond,
+                                 double rearLeftMetersPerSecond,
+                                 double rearRightMetersPerSecond) {
+    this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
+    this.frontRightMetersPerSecond = frontRightMetersPerSecond;
+    this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
+    this.rearRightMetersPerSecond = rearRightMetersPerSecond;
+  }
+
+  /**
+   * 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.
+   *
+   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond,
+        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond)
+        .max().getAsDouble();
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+      rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed
+          * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
+        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
new file mode 100644
index 0000000..a1dba43
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
@@ -0,0 +1,199 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import java.util.Arrays;
+import java.util.Collections;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual module states (speed and angle).
+ *
+ * <p>The inverse kinematics (converting from a desired chassis velocity to
+ * individual module states) uses the relative locations of the modules with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion maneuvers.
+ *
+ * <p>Forward kinematics (converting an array of module states into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
+ * multiply by [moduleStates] to get our chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+public class SwerveDriveKinematics {
+  private final SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final int m_numModules;
+  private final Translation2d[] m_modules;
+  private Translation2d m_prevCoR = new Translation2d();
+
+  /**
+   * Constructs a swerve drive kinematics object. This takes in a variable
+   * number of wheel locations as Translation2ds. The order in which you pass in
+   * the wheel locations is the same order that you will receive the module
+   * states when performing inverse kinematics. It is also expected that you
+   * pass in the module states in the same order when calling the forward
+   * kinematics methods.
+   *
+   * @param wheelsMeters The locations of the wheels relative to the physical center
+   *                     of the robot.
+   */
+  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
+    if (wheelsMeters.length < 2) {
+      throw new IllegalArgumentException("A swerve drive requires at least two modules");
+    }
+    m_numModules = wheelsMeters.length;
+    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+    m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
+
+    for (int i = 0; i < m_numModules; i++) {
+      m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
+      m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
+    }
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
+  }
+
+  /**
+   * Performs inverse kinematics to return the module states from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * module speeds and angles.
+   *
+   * <p>This function also supports variable centers of rotation. During normal
+   * operations, the center of rotation is usually the same as the physical
+   * center of the robot; therefore, the argument is defaulted to that use case.
+   * However, if you wish to change the center of rotation for evasive
+   * maneuvers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds    The desired chassis speed.
+   * @param centerOfRotationMeters The center of rotation. For example, if you set the
+   *                         center of rotation at one corner of the robot and provide
+   *                         a chassis speed that only has a dtheta 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.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
+                                                  Translation2d centerOfRotationMeters) {
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      for (int i = 0; i < m_numModules; i++) {
+        m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
+            -m_modules[i].getY() + centerOfRotationMeters.getY());
+        m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1,
+            +m_modules[i].getX() - centerOfRotationMeters.getX());
+      }
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(0, 0,
+        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond);
+
+    var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
+
+    for (int i = 0; i < m_numModules; i++) {
+      double x = moduleStatesMatrix.get(i * 2, 0);
+      double y = moduleStatesMatrix.get(i * 2 + 1, 0);
+
+      double speed = Math.hypot(x, y);
+      Rotation2d angle = new Rotation2d(x, y);
+
+      moduleStates[i] = new SwerveModuleState(speed, angle);
+    }
+
+    return moduleStates;
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
+   * toSwerveModuleStates for more information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return An array containing the module states.
+   */
+  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
+    return toSwerveModuleStates(chassisSpeeds, new Translation2d());
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the
+   * given module states. This method is often used for odometry -- determining
+   * the robot's position on the field using data from the real-world speed and
+   * angle of each module on the robot.
+   *
+   * @param wheelStates The state of the modules (as a SwerveModuleState type)
+   *                    as measured from respective encoders and gyros. The order of the swerve
+   *                    module states should be same as passed into the constructor of this class.
+   * @return The resulting chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
+    if (wheelStates.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor"
+      );
+    }
+    var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = wheelStates[i];
+      moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
+      moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
+    }
+
+    var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
+    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.get(2, 0));
+
+  }
+
+  /**
+   * 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.
+   *
+   * @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(SwerveModuleState[] moduleStates,
+                                          double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      for (SwerveModuleState moduleState : moduleStates) {
+        moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed
+            * attainableMaxSpeedMetersPerSecond;
+      }
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
new file mode 100644
index 0000000..5b1f975
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Twist2d;
+import edu.wpi.first.wpiutil.WPIUtilJNI;
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * swerve drive encoders and swerve azimuth encoders.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+public class SwerveDriveOdometry {
+  private final SwerveDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a SwerveDriveOdometry object.
+   *
+   * @param kinematics  The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle   The angle reported by the gyroscope.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
+                             Pose2d initialPose) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPose;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPose.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
+  }
+
+  /**
+   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle  The angle reported by the gyroscope.
+   */
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>The gyroscope angle does not need to be reset here on the user's robot code.
+   * The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose      The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+    m_poseMeters = pose;
+    m_previousAngle = pose.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param currentTimeSeconds The current time in seconds.
+   * @param gyroAngle          The angle reported by the gyroscope.
+   * @param moduleStates       The current state of all swerve modules. Please provide
+   *                           the states in the same order in which you instantiated your
+   *                           SwerveDriveKinematics.
+   * @return The new pose of the robot.
+   */
+  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
+                               SwerveModuleState... moduleStates) {
+    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
+    var newPose = m_poseMeters.exp(
+        new Twist2d(chassisState.vxMetersPerSecond * period,
+            chassisState.vyMetersPerSecond * period,
+            angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method automatically calculates the
+   * current time to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the angular
+   * rate that is calculated from forward kinematics.
+   *
+   * @param gyroAngle    The angle reported by the gyroscope.
+   * @param moduleStates The current state of all swerve modules. Please provide
+   *                     the states in the same order in which you instantiated your
+   *                     SwerveDriveKinematics.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
new file mode 100644
index 0000000..f9570eb
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents the state of one swerve module.
+ */
+@SuppressWarnings("MemberName")
+public class SwerveModuleState implements Comparable<SwerveModuleState> {
+
+  /**
+   * Speed of the wheel of the module.
+   */
+  public double speedMetersPerSecond;
+
+  /**
+   * Angle of the module.
+   */
+  public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+  /**
+   * Constructs a SwerveModuleState with zeros for speed and angle.
+   */
+  public SwerveModuleState() {
+  }
+
+  /**
+   * Constructs a SwerveModuleState.
+   *
+   * @param speedMetersPerSecond The speed of the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
+    this.speedMetersPerSecond = speedMetersPerSecond;
+    this.angle = angle;
+  }
+
+  /**
+   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
+   * is higher than the other.
+   *
+   * @param o The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  @SuppressWarnings("ParameterName")
+  public int compareTo(SwerveModuleState o) {
+    return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
+        angle);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java
new file mode 100644
index 0000000..ad9bf27
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java
@@ -0,0 +1,179 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.math;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.Pair;
+
+@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+public final class Discretization {
+  private Discretization() {
+    // Utility class
+  }
+
+  /**
+   * Discretizes the given continuous A matrix.
+   *
+   * @param <States>       Num representing the number of states.
+   * @param contA     Continuous system matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return the discrete matrix system.
+   */
+  public static <States extends Num> Matrix<States, States> discretizeA(
+      Matrix<States, States> contA, double dtSeconds) {
+    return contA.times(dtSeconds).exp();
+  }
+
+  /**
+   * Discretizes the given continuous A and B matrices.
+   *
+   * <p>Rather than solving a (States + Inputs) x (States + Inputs) matrix
+   * exponential like in DiscretizeAB(), we take advantage of the structure of the
+   * block matrix of A and B.
+   *
+   * <p>1) The exponential of A*t, which is only N x N, is relatively cheap.
+   * 2) The upper-right quarter of the (States + Inputs) x (States + Inputs)
+   * matrix, which we can approximate using a taylor series to several terms
+   * and still be substantially cheaper than taking the big exponential.
+   *
+   * @param states    Nat representing the states of the system.
+   * @param contA     Continuous system matrix.
+   * @param contB     Continuous input matrix.
+   * @param dtseconds Discretization timestep.
+   */
+  public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
+      Matrix<States, Inputs>>
+      discretizeABTaylor(Nat<States> states,
+                         Matrix<States, States> contA,
+                         Matrix<States, Inputs> contB,
+                         double dtseconds) {
+    Matrix<States, States> lastTerm = Matrix.eye(states);
+    double lastCoeff = dtseconds;
+
+    var phi12 = lastTerm.times(lastCoeff);
+
+    // i = 6 i.e. 5th order should be enough precision
+    for (int i = 2; i < 6; ++i) {
+      lastTerm = contA.times(lastTerm);
+      lastCoeff *= dtseconds / ((double) i);
+
+      phi12 = phi12.plus(lastTerm.times(lastCoeff));
+    }
+
+    var discB = phi12.times(contB);
+
+    var discA = discretizeA(contA, dtseconds);
+
+    return Pair.of(discA, discB);
+  }
+
+  /**
+   * Discretizes the given continuous A and Q matrices.
+   *
+   * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which
+   * is expensive), we take advantage of the structure of the block matrix of A
+   * and Q.
+   *
+   * <p>The exponential of A*t, which is only N x N, is relatively cheap.
+   * 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
+   * using a taylor series to several terms and still be substantially cheaper
+   * than taking the big exponential.
+   *
+   * @param <States>       Nat representing the number of states.
+   * @param contA     Continuous system matrix.
+   * @param contQ     Continuous process noise covariance matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a pair representing the discrete system matrix and process noise covariance matrix.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static <States extends Num> Pair<Matrix<States, States>,
+            Matrix<States, States>> discretizeAQTaylor(Matrix<States, States> contA,
+                                                       Matrix<States, States> contQ,
+                                                       double dtSeconds) {
+    Matrix<States, States> Q = (contQ.plus(contQ.transpose())).div(2.0);
+
+
+    Matrix<States, States> lastTerm = Q.copy();
+    double lastCoeff = dtSeconds;
+
+    // A^T^n
+    Matrix<States, States> Atn = contA.transpose();
+    Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
+
+    // i = 6 i.e. 6th order should be enough precision
+    for (int i = 2; i < 6; ++i) {
+      lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn));
+      lastCoeff *= dtSeconds / ((double) i);
+
+      phi12 = phi12.plus(lastTerm.times(lastCoeff));
+
+      Atn = Atn.times(contA.transpose());
+    }
+
+    var discA = discretizeA(contA, dtSeconds);
+    Q = discA.times(phi12);
+
+    // Make Q symmetric if it isn't already
+    var discQ = Q.plus(Q.transpose()).div(2.0);
+
+    return new Pair<>(discA, discQ);
+  }
+
+  /**
+   * Returns a discretized version of the provided continuous measurement noise
+   * covariance matrix. Note that dt=0.0 divides R by zero.
+   *
+   * @param <O>       Nat representing the number of outputs.
+   * @param R         Continuous measurement noise covariance matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return Discretized version of the provided continuous measurement noise covariance matrix.
+   */
+  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
+    return R.div(dtSeconds);
+  }
+
+  /**
+   * Discretizes the given continuous A and B matrices.
+   *
+   * @param <States>       Nat representing the states of the system.
+   * @param <Inputs>       Nat representing the inputs to the system.
+   * @param contA     Continuous system matrix.
+   * @param contB     Continuous input matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a Pair representing discA and diskB.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
+            Matrix<States, Inputs>> discretizeAB(
+                                     Matrix<States, States> contA,
+                                     Matrix<States, Inputs> contB,
+                                     double dtSeconds) {
+    var scaledA = contA.times(dtSeconds);
+    var scaledB = contB.times(dtSeconds);
+
+    var contSize = contB.getNumRows() + contB.getNumCols();
+    var Mcont = new Matrix<>(new SimpleMatrix(contSize, contSize));
+    Mcont.assignBlock(0, 0, scaledA);
+    Mcont.assignBlock(0, scaledA.getNumCols(), scaledB);
+    var Mdisc = Mcont.exp();
+
+    var discA = new Matrix<States, States>(new SimpleMatrix(contB.getNumRows(),
+        contB.getNumRows()));
+    var discB = new Matrix<States, Inputs>(new SimpleMatrix(contB.getNumRows(),
+        contB.getNumCols()));
+
+    discA.extractFrom(0, 0, Mdisc);
+    discB.extractFrom(0, contB.getNumRows(), Mdisc);
+
+    return new Pair<>(discA, discB);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java
new file mode 100644
index 0000000..02ca7c1
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java
@@ -0,0 +1,180 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.math;
+
+import java.util.Random;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpiutil.math.MathUtil;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+
+@SuppressWarnings("ParameterName")
+public final class StateSpaceUtil {
+  private StateSpaceUtil() {
+    // Utility class
+  }
+
+  /**
+   * Creates a covariance matrix from the given vector for use with Kalman
+   * filters.
+   *
+   * <p>Each element is squared and placed on the covariance matrix diagonal.
+   *
+   * @param <States>     Num representing the states of the system.
+   * @param states  A Nat representing the states of the system.
+   * @param stdDevs For a Q matrix, its elements are the standard deviations of
+   *                each state from how the model behaves. For an R matrix, its
+   *                elements are the standard deviations for each output
+   *                measurement.
+   * @return Process noise or measurement noise covariance matrix.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
+      Nat<States> states, Matrix<States, N1> stdDevs
+  ) {
+    var result = new Matrix<>(states, states);
+    for (int i = 0; i < states.getNum(); i++) {
+      result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
+    }
+    return result;
+  }
+
+  /**
+   * Creates a vector of normally distributed white noise with the given noise
+   * intensities for each element.
+   *
+   * @param <N>     Num representing the dimensionality of  the noise vector to create.
+   * @param stdDevs A matrix whose elements are the standard deviations of each
+   *                element of the noise vector.
+   * @return White noise vector.
+   */
+  public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(
+        Matrix<N, N1> stdDevs
+  ) {
+    var rand = new Random();
+
+    Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
+    for (int i = 0; i < stdDevs.getNumRows(); i++) {
+      result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
+    }
+    return result;
+  }
+
+  /**
+   * Creates a cost matrix from the given vector for use with LQR.
+   *
+   * <p>The cost matrix is constructed using Bryson's rule. The inverse square of
+   * each element in the input is taken and placed on the cost matrix diagonal.
+   *
+   * @param <States>   Nat representing the states of the system.
+   * @param costs An array. For a Q matrix, its elements are the maximum allowed
+   *              excursions of the states from the reference. For an R matrix,
+   *              its elements are the maximum allowed excursions of the control
+   *              inputs from no actuation.
+   * @return State excursion or control effort cost matrix.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num> Matrix<States, States>
+      makeCostMatrix(Matrix<States, N1> costs) {
+    Matrix<States, States> result =
+        new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
+    result.fill(0.0);
+
+    for (int i = 0; i < costs.getNumRows(); i++) {
+      result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2)));
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns true if (A, B) is a stabilizable pair.
+   *
+   * <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+   * any, have absolute values less than one, where an eigenvalue is
+   * uncontrollable if rank(lambda * I - A, B) %3C n where n is number of states.
+   *
+   * @param <States> Num representing the size of A.
+   * @param <Inputs> Num representing the columns of B.
+   * @param A   System matrix.
+   * @param B   Input matrix.
+   * @return If the system is stabilizable.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> boolean isStabilizable(
+      Matrix<States, States> A, Matrix<States, Inputs> B) {
+    return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(),
+            A.getData(), B.getData());
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
+   *
+   * @param pose A pose to convert to a vector.
+   * @return The given pose in vector form, with the third element, theta, in radians.
+   */
+  public static Matrix<N3, N1> poseToVector(Pose2d pose) {
+    return VecBuilder.fill(
+            pose.getX(),
+            pose.getY(),
+            pose.getRotation().getRadians()
+    );
+  }
+
+  /**
+   * Clamp the input u to the min and max.
+   *
+   * @param u    The input to clamp.
+   * @param umin The minimum input magnitude.
+   * @param umax The maximum input magnitude.
+   * @param <I>  The number of inputs.
+   * @return     The clamped input.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(Matrix<I, N1> u,
+                                                                     Matrix<I, N1> umin,
+                                                                     Matrix<I, N1> umax) {
+    var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
+    for (int i = 0; i < u.getNumRows(); i++) {
+      result.set(i, 0, MathUtil.clamp(
+            u.get(i, 0),
+            umin.get(i, 0),
+            umax.get(i, 0)));
+    }
+    return result;
+  }
+
+  /**
+   * Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
+   * differential drivetrains.
+   *
+   * @param u            The input vector.
+   * @param maxMagnitude The maximum magnitude any input can have.
+   * @param <I>          The number of inputs.
+   * @return The normalizedInput
+   */
+  public static <I extends Num> Matrix<I, N1> normalizeInputVector(Matrix<I, N1> u,
+                                                                   double maxMagnitude) {
+    double maxValue = u.maxAbs();
+    boolean isCapped = maxValue > maxMagnitude;
+
+    if (isCapped) {
+      return u.times(maxMagnitude / maxValue);
+    }
+    return u;
+  }
+
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
new file mode 100644
index 0000000..f387fc0
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class CubicHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * Constructs a cubic hermite spline with the specified control vectors. Each
+   * control vector contains info about the location of the point and its first
+   * derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   *                              the x dimension.
+   * @param xFinalControlVector   The control vector for the final point in
+   *                              the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   *                              the y dimension.
+   * @param yFinalControlVector   The control vector for the final point in
+   *                              the y dimension.
+   */
+  @SuppressWarnings("ParameterName")
+  public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
+                            double[] yInitialControlVector, double[] yFinalControlVector) {
+    super(3);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 4);
+
+    for (int i = 0; i < 4; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // Then populate row 4 and 5 with the second derivatives.
+      // Here, we are multiplying by (3 - i) to manually take the derivative. The
+      // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(2, i, m_coefficients.get(0, i) * (3 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i));
+    }
+
+    for (int i = 0; i < 3; i++) {
+      // Here, we are multiplying by (2 - i) to manually take the derivative. The
+      // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
+    }
+
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for cubic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for cubic hermite spline interpolation.
+   */
+  private SimpleMatrix makeHermiteBasis() {
+    if (hermiteBasis == null) {
+      hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{
+          +2.0, +1.0, -2.0, +1.0,
+          -3.0, -2.0, +3.0, -1.0,
+          +0.0, +1.0, +0.0, +0.0,
+          +1.0, +0.0, +0.0, +0.0
+      });
+    }
+    return hermiteBasis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector   The control vector for the final point.
+   * @return The control vector matrix for a dimension.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 2 || finalVector.length != 2) {
+      throw new IllegalArgumentException("Size of vectors must be 2");
+    }
+    return new SimpleMatrix(4, 1, true, new double[]{
+        initialVector[0], initialVector[1],
+        finalVector[0], finalVector[1]});
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
new file mode 100644
index 0000000..ed8562d
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * Represents a pair of a pose and a curvature.
+ */
+@SuppressWarnings("MemberName")
+public class PoseWithCurvature {
+  // Represents the pose.
+  public Pose2d poseMeters;
+
+  // Represents the curvature.
+  public double curvatureRadPerMeter;
+
+  /**
+   * Constructs a PoseWithCurvature.
+   *
+   * @param poseMeters           The pose.
+   * @param curvatureRadPerMeter The curvature.
+   */
+  public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
+    this.poseMeters = poseMeters;
+    this.curvatureRadPerMeter = curvatureRadPerMeter;
+  }
+
+  /**
+   * Constructs a PoseWithCurvature with default values.
+   */
+  public PoseWithCurvature() {
+    poseMeters = new Pose2d();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
new file mode 100644
index 0000000..6073f62
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class QuinticHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * Constructs a quintic hermite spline with the specified control vectors.
+   * Each control vector contains into about the location of the point, its
+   * first derivative, and its second derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   *                              the x dimension.
+   * @param xFinalControlVector   The control vector for the final point in
+   *                              the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   *                              the y dimension.
+   * @param yFinalControlVector   The control vector for the final point in
+   *                              the y dimension.
+   */
+  @SuppressWarnings("ParameterName")
+  public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
+                              double[] yInitialControlVector, double[] yFinalControlVector) {
+    super(5);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 6);
+
+    for (int i = 0; i < 6; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+    }
+    for (int i = 0; i < 6; i++) {
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // Here, we are multiplying by (5 - i) to manually take the derivative. The
+      // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
+    }
+    for (int i = 0; i < 5; i++) {
+      // Then populate row 4 and 5 with the second derivatives.
+      // Here, we are multiplying by (4 - i) to manually take the derivative. The
+      // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+      // coefficient of the derivative, we can use the power rule and multiply
+      // the existing coefficient by its power.
+      m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
+    }
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for quintic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for quintic hermite spline interpolation.
+   */
+  private SimpleMatrix makeHermiteBasis() {
+    if (hermiteBasis == null) {
+      hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{
+          -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+          +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+          -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+          +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+          +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+          +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
+      });
+    }
+    return hermiteBasis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector   The control vector for the final point.
+   * @return The control vector matrix for a dimension.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 3 || finalVector.length != 3) {
+      throw new IllegalArgumentException("Size of vectors must be 3");
+    }
+    return new SimpleMatrix(6, 1, true, new double[]{
+        initialVector[0], initialVector[1], initialVector[2],
+        finalVector[0], finalVector[1], finalVector[2]});
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
new file mode 100644
index 0000000..57c388f
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.Arrays;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * Represents a two-dimensional parametric spline that interpolates between two
+ * points.
+ */
+public abstract class Spline {
+  private final int m_degree;
+
+  /**
+   * Constructs a spline with the given degree.
+   *
+   * @param degree The degree of the spline.
+   */
+  Spline(int degree) {
+    m_degree = degree;
+  }
+
+  /**
+   * Returns the coefficients of the spline.
+   *
+   * @return The coefficients of the spline.
+   */
+  protected abstract SimpleMatrix getCoefficients();
+
+  /**
+   * Gets the pose and curvature at some point t on the spline.
+   *
+   * @param t The point t
+   * @return The pose and curvature at that point.
+   */
+  @SuppressWarnings("ParameterName")
+  public PoseWithCurvature getPoint(double t) {
+    SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
+    final var coefficients = getCoefficients();
+
+    // Populate the polynomial bases.
+    for (int i = 0; i <= m_degree; i++) {
+      polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
+    }
+
+    // This simply multiplies by the coefficients. We need to divide out t some
+    // n number of times where n is the derivative we want to take.
+    SimpleMatrix combined = coefficients.mult(polynomialBases);
+
+    // Get x and y
+    final double x = combined.get(0, 0);
+    final double y = combined.get(1, 0);
+
+    double dx;
+    double dy;
+    double ddx;
+    double ddy;
+
+    if (t == 0) {
+      dx = coefficients.get(2, m_degree - 1);
+      dy = coefficients.get(3, m_degree - 1);
+      ddx = coefficients.get(4, m_degree - 2);
+      ddy = coefficients.get(5, m_degree - 2);
+    } else {
+      // Divide out t once for first derivative.
+      dx = combined.get(2, 0) / t;
+      dy = combined.get(3, 0) / t;
+
+      // Divide out t twice for second derivative.
+      ddx = combined.get(4, 0) / t / t;
+      ddy = combined.get(5, 0) / t / t;
+    }
+
+    // Find the curvature.
+    final double curvature =
+        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
+
+    return new PoseWithCurvature(
+        new Pose2d(x, y, new Rotation2d(dx, dy)),
+        curvature
+    );
+  }
+
+  /**
+   * Represents a control vector for a spline.
+   *
+   * <p>Each element in each array represents the value of the derivative at the index. For
+   * example, the value of x[2] is the second derivative in the x dimension.
+   */
+  @SuppressWarnings("MemberName")
+  public static class ControlVector {
+    public double[] x;
+    public double[] y;
+
+    /**
+     * Instantiates a control vector.
+     * @param x The x dimension of the control vector.
+     * @param y The y dimension of the control vector.
+     */
+    @SuppressWarnings("ParameterName")
+    public ControlVector(double[] x, double[] y) {
+      this.x = Arrays.copyOf(x, x.length);
+      this.y = Arrays.copyOf(y, y.length);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
new file mode 100644
index 0000000..a2bf9dd
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
@@ -0,0 +1,280 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.Arrays;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+public final class SplineHelper {
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private SplineHelper() {
+  }
+
+  /**
+   * Returns 2 cubic control vectors from a set of exterior waypoints and
+   * interior translations.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @return 2 cubic control vectors.
+   */
+  public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
+      Pose2d start, Translation2d[] interiorWaypoints, Pose2d end
+  ) {
+    // Generate control vectors from poses.
+    Spline.ControlVector initialCV;
+    Spline.ControlVector endCV;
+
+    // Chooses a magnitude automatically that makes the splines look better.
+    if (interiorWaypoints.length < 1) {
+      double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      endCV = getCubicControlVector(scalar, end);
+    } else {
+      double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1])
+          * 1.2;
+      endCV = getCubicControlVector(scalar, end);
+    }
+    return new Spline.ControlVector[]{initialCV, endCV};
+  }
+
+  /**
+   * Returns quintic splines from a set of waypoints.
+   *
+   * @param waypoints The waypoints
+   * @return List of splines.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
+    for (int i = 0; i < waypoints.size() - 1; ++i) {
+      var p0 = waypoints.get(i);
+      var p1 = waypoints.get(i + 1);
+
+      // This just makes the splines look better.
+      final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
+
+      var controlVecA = getQuinticControlVector(scalar, p0);
+      var controlVecB = getQuinticControlVector(scalar, p1);
+
+      splines[i]
+          = new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
+    }
+    return splines;
+  }
+
+  /**
+   * Returns a set of cubic splines corresponding to the provided control vectors. The
+   * user is free to set the direction of the start and end point. The
+   * directions for the middle waypoints are determined automatically to ensure
+   * continuous curvature throughout the path.
+   *
+   * @param start     The starting control vector.
+   * @param waypoints The middle waypoints. This can be left blank if you only
+   *                  wish to create a path with two waypoints.
+   * @param end       The ending control vector.
+   * @return A vector of cubic hermite splines that interpolate through the
+   *         provided waypoints and control vectors.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
+                        "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
+      Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
+
+    CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
+
+    double[] xInitial = start.x;
+    double[] yInitial = start.y;
+    double[] xFinal = end.x;
+    double[] yFinal = end.y;
+
+    if (waypoints.length > 1) {
+      Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
+
+      // Create an array of all waypoints, including the start and end.
+      newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
+      System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
+      newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], yFinal[0]);
+
+      // Populate tridiagonal system for clamped cubic
+      /* See:
+      https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
+      /undervisningsmateriale/chap7alecture.pdf
+      */
+      // Above-diagonal of tridiagonal matrix, zero-padded
+      final double[] a = new double[newWaypts.length - 2];
+
+      // Diagonal of tridiagonal matrix
+      final double[] b = new double[newWaypts.length - 2];
+      Arrays.fill(b, 4.0);
+
+      // Below-diagonal of tridiagonal matrix, zero-padded
+      final double[] c = new double[newWaypts.length - 2];
+
+      // rhs vectors
+      final double[] dx = new double[newWaypts.length - 2];
+      final double[] dy = new double[newWaypts.length - 2];
+
+      // solution vectors
+      final double[] fx = new double[newWaypts.length - 2];
+      final double[] fy = new double[newWaypts.length - 2];
+
+      // populate above-diagonal and below-diagonal vectors
+      a[0] = 0.0;
+      for (int i = 0; i < newWaypts.length - 3; i++) {
+        a[i + 1] = 1;
+        c[i] = 1;
+      }
+      c[c.length - 1] = 0.0;
+
+      // populate rhs vectors
+      dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
+      dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
+
+      if (newWaypts.length > 4) {
+        for (int i = 1; i <= newWaypts.length - 4; i++) {
+          // dx and dy represent the derivatives of the internal waypoints. The derivative
+          // of the second internal waypoint should involve the third and first internal waypoint,
+          // which have indices of 1 and 3 in the newWaypts list (which contains ALL waypoints).
+          dx[i] = 3 * (newWaypts[i + 2].getX() - newWaypts[i].getX());
+          dy[i] = 3 * (newWaypts[i + 2].getY() - newWaypts[i].getY());
+        }
+      }
+
+      dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
+          - newWaypts[newWaypts.length - 3].getX()) - xFinal[1];
+      dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
+          - newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
+
+      // Compute solution to tridiagonal system
+      thomasAlgorithm(a, b, c, dx, fx);
+      thomasAlgorithm(a, b, c, dy, fy);
+
+      double[] newFx = new double[fx.length + 2];
+      double[] newFy = new double[fy.length + 2];
+
+      newFx[0] = xInitial[1];
+      newFy[0] = yInitial[1];
+      System.arraycopy(fx, 0, newFx, 1, fx.length);
+      System.arraycopy(fy, 0, newFy, 1, fy.length);
+      newFx[newFx.length - 1] = xFinal[1];
+      newFy[newFy.length - 1] = yFinal[1];
+
+      for (int i = 0; i < newFx.length - 1; i++) {
+        splines[i] = new CubicHermiteSpline(
+            new double[]{newWaypts[i].getX(), newFx[i]},
+            new double[]{newWaypts[i + 1].getX(), newFx[i + 1]},
+            new double[]{newWaypts[i].getY(), newFy[i]},
+            new double[]{newWaypts[i + 1].getY(), newFy[i + 1]}
+        );
+      }
+    } else if (waypoints.length == 1) {
+      final var xDeriv = (3 * (xFinal[0]
+          - xInitial[0])
+          - xFinal[1] - xInitial[1])
+          / 4.0;
+      final var yDeriv = (3 * (yFinal[0]
+          - yInitial[0])
+          - yFinal[1] - yInitial[1])
+          / 4.0;
+
+      double[] midXControlVector = {waypoints[0].getX(), xDeriv};
+      double[] midYControlVector = {waypoints[0].getY(), yDeriv};
+
+      splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
+                                          yInitial, midYControlVector);
+      splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
+                                          midYControlVector, yFinal);
+    } else {
+      splines[0] = new CubicHermiteSpline(xInitial, xFinal,
+                                          yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * Returns a set of quintic splines corresponding to the provided control vectors.
+   * The user is free to set the direction of all control vectors. Continuous
+   * curvature is guaranteed throughout the path.
+   *
+   * @param controlVectors The control vectors.
+   * @return A vector of quintic hermite splines that interpolate through the
+   *         provided waypoints.
+   */
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
+      Spline.ControlVector[] controlVectors) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
+    for (int i = 0; i < controlVectors.length - 1; i++) {
+      var xInitial = controlVectors[i].x;
+      var xFinal = controlVectors[i + 1].x;
+      var yInitial = controlVectors[i].y;
+      var yFinal = controlVectors[i + 1].y;
+      splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
+                                            yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * Thomas algorithm for solving tridiagonal systems Af = d.
+   *
+   * @param a              the values of A above the diagonal
+   * @param b              the values of A on the diagonal
+   * @param c              the values of A below the diagonal
+   * @param d              the vector on the rhs
+   * @param solutionVector the unknown (solution) vector, modified in-place
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  private static void thomasAlgorithm(double[] a, double[] b,
+                                      double[] c, double[] d, double[] solutionVector) {
+    int N = d.length;
+
+    double[] cStar = new double[N];
+    double[] dStar = new double[N];
+
+    // This updates the coefficients in the first row
+    // Note that we should be checking for division by zero here
+    cStar[0] = c[0] / b[0];
+    dStar[0] = d[0] / b[0];
+
+    // Create the c_star and d_star coefficients in the forward sweep
+    for (int i = 1; i < N; i++) {
+      double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
+      cStar[i] = c[i] * m;
+      dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
+    }
+    solutionVector[N - 1] = dStar[N - 1];
+    // This is the reverse sweep, used to update the solution vector f
+    for (int i = N - 2; i >= 0; i--) {
+      solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
+    }
+  }
+
+  private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[]{point.getX(), scalar * point.getRotation().getCos()},
+        new double[]{point.getY(), scalar * point.getRotation().getSin()}
+    );
+  }
+
+  private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[]{point.getX(), scalar * point.getRotation().getCos(), 0.0},
+        new double[]{point.getY(), scalar * point.getRotation().getSin(), 0.0}
+    );
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
new file mode 100644
index 0000000..1585214
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
@@ -0,0 +1,153 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Class used to parameterize a spline by its arc length.
+ */
+public final class SplineParameterizer {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  /**
+   * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
+   * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
+   * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
+   * this maximum should definitely indicate something has gone wrong.
+   */
+  private static final int kMaxIterations = 5000;
+
+  @SuppressWarnings("MemberName")
+  private static class StackContents {
+    final double t1;
+    final double t0;
+
+    StackContents(double t0, double t1) {
+      this.t0 = t0;
+      this.t1 = t1;
+    }
+  }
+
+  @SuppressWarnings("serial")
+  public static class MalformedSplineException extends RuntimeException {
+    /**
+     * Create a new exception with the given message.
+     *
+     * @param message the message to pass with the exception
+     */
+    private MalformedSplineException(String message) {
+      super(message);
+    }
+  }
+
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private SplineParameterizer() {
+  }
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various
+   * arcs until their dx, dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @return A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> parameterize(Spline spline) {
+    return parameterize(spline, 0.0, 1.0);
+  }
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various
+   * arcs until their dx, dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @param t0     Starting internal spline parameter. It is recommended to use 0.0.
+   * @param t1     Ending internal spline parameter. It is recommended to use 1.0.
+   * @return       A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.add(spline.getPoint(t0));
+
+    // We use an "explicit stack" to simulate recursion, instead of a recursive function call
+    // This give us greater control, instead of a stack overflow
+    var stack = new ArrayDeque<StackContents>();
+    stack.push(new StackContents(t0, t1));
+
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
+
+    while (!stack.isEmpty()) {
+      current = stack.removeFirst();
+      start = spline.getPoint(current.t0);
+      end = spline.getPoint(current.t1);
+
+      final var twist = start.poseMeters.log(end.poseMeters);
+      if (
+          Math.abs(twist.dy) > kMaxDy
+          || Math.abs(twist.dx) > kMaxDx
+          || Math.abs(twist.dtheta) > kMaxDtheta
+      ) {
+        stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
+        stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
+      } else {
+        splinePoints.add(spline.getPoint(current.t1));
+      }
+
+      iterations++;
+      if (iterations >= kMaxIterations) {
+        throw new MalformedSplineException(
+          "Could not parameterize a malformed spline. "
+          + "This means that you probably had two or more adjacent waypoints that were very close "
+          + "together with headings in opposing directions."
+        );
+      }
+    }
+
+    return splinePoints;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java
new file mode 100644
index 0000000..4a90caa
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system;
+
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+@SuppressWarnings("ClassTypeParameterName")
+public class LinearSystem<States extends Num, Inputs extends Num,
+        Outputs extends Num> {
+
+  /**
+   * Continuous system matrix.
+   */
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, States> m_A;
+
+  /**
+   * Continuous input matrix.
+   */
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Inputs> m_B;
+
+  /**
+   * Output matrix.
+   */
+  @SuppressWarnings("MemberName")
+  private final Matrix<Outputs, States> m_C;
+
+  /**
+   * Feedthrough matrix.
+   */
+  @SuppressWarnings("MemberName")
+  private final Matrix<Outputs, Inputs> m_D;
+
+  /**
+   * Construct a new LinearSystem from the four system matrices.
+   *
+   * @param a             The system matrix A.
+   * @param b             The input matrix B.
+   * @param c             The output matrix C.
+   * @param d             The feedthrough matrix D.
+   */
+  @SuppressWarnings("ParameterName")
+  public LinearSystem(Matrix<States, States> a, Matrix<States, Inputs> b,
+                      Matrix<Outputs, States> c, Matrix<Outputs, Inputs> d) {
+
+    this.m_A = a;
+    this.m_B = b;
+    this.m_C = c;
+    this.m_D = d;
+  }
+
+  /**
+   * Returns the system matrix A.
+   *
+   * @return the system matrix A.
+   */
+  public Matrix<States, States> getA() {
+    return m_A;
+  }
+
+  /**
+   * Returns an element of the system matrix A.
+   *
+   * @param row Row of A.
+   * @param col Column of A.
+   * @return the system matrix A at (i, j).
+   */
+  public double getA(int row, int col) {
+    return m_A.get(row, col);
+  }
+
+  /**
+   * Returns the input matrix B.
+   *
+   * @return the input matrix B.
+   */
+  public Matrix<States, Inputs> getB() {
+    return m_B;
+  }
+
+  /**
+   * Returns an element of the input matrix B.
+   *
+   * @param row Row of B.
+   * @param col Column of B.
+   * @return The value of the input matrix B at (i, j).
+   */
+  public double getB(int row, int col) {
+    return m_B.get(row, col);
+  }
+
+  /**
+   * Returns the output matrix C.
+   *
+   * @return Output matrix C.
+   */
+  public Matrix<Outputs, States> getC() {
+    return m_C;
+  }
+
+  /**
+   * Returns an element of the output matrix C.
+   *
+   * @param row Row of C.
+   * @param col Column of C.
+   * @return the double value of C at the given position.
+   */
+  public double getC(int row, int col) {
+    return m_C.get(row, col);
+  }
+
+  /**
+   * Returns the feedthrough matrix D.
+   *
+   * @return the feedthrough matrix D.
+   */
+  public Matrix<Outputs, Inputs> getD() {
+    return m_D;
+  }
+
+  /**
+   * Returns an element of the feedthrough matrix D.
+   *
+   * @param row Row of D.
+   * @param col Column of D.
+   * @return The feedthrough matrix D at (i, j).
+   */
+  public double getD(int row, int col) {
+    return m_D.get(row, col);
+  }
+
+  /**
+   * Computes the new x given the old x and the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state
+   * estimate.
+   *
+   * @param x         The current state.
+   * @param clampedU  The control input.
+   * @param dtSeconds Timestep for model update.
+   * @return the updated x.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<States, N1> calculateX(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU,
+                                       double dtSeconds) {
+    var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
+
+    return (discABpair.getFirst().times(x)).plus(discABpair.getSecond().times(clampedU));
+  }
+
+  /**
+   * Computes the new y given the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state
+   * estimate.
+   *
+   * @param x The current state.
+   * @param clampedU The control input.
+   * @return the updated output matrix Y.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Outputs, N1> calculateY(
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> clampedU) {
+    return m_C.times(x).plus(m_D.times(clampedU));
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n", m_A.toString(),
+            m_B.toString(), m_C.toString(), m_D.toString());
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
new file mode 100644
index 0000000..d44ca62
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
@@ -0,0 +1,358 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system;
+
+import java.util.function.Function;
+
+import org.ejml.MatrixDimensionException;
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
+import edu.wpi.first.wpilibj.estimator.KalmanFilter;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * Combines a plant, controller, and observer for controlling a mechanism with
+ * full state feedback.
+ *
+ * <p>For everything in this file, "inputs" and "outputs" are defined from the
+ * perspective of the plant. This means U is an input and Y is an output
+ * (because you give the plant U (powers) and it gives you back a Y (sensor
+ * values). This is the opposite of what they mean from the perspective of the
+ * controller (U is an output because that's what goes to the motors and Y is an
+ * input because that's what comes back from the sensors).
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class LinearSystemLoop<States extends Num, Inputs extends Num,
+        Outputs extends Num> {
+
+  private final LinearSystem<States, Inputs, Outputs> m_plant;
+  private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
+  private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
+  private final KalmanFilter<States, Inputs, Outputs> m_observer;
+  private Matrix<States, N1> m_nextR;
+  private Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> m_clampFunction;
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer. By default, the initial reference is all zeros. Users should
+   * call reset with the initial system state before enabling the loop. This
+   * constructor assumes that the input(s) to this system are voltage.
+   *
+   * @param plant      State-space plant.
+   * @param controller State-space controller.
+   * @param observer   State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12.
+   * @param dtSeconds The nominal timestep.
+   */
+  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
+                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+                          KalmanFilter<States, Inputs, Outputs> observer,
+                          double maxVoltageVolts,
+                          double dtSeconds) {
+    this(plant, controller,
+        new LinearPlantInversionFeedforward<>(plant, dtSeconds), observer,
+        u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer. By default, the initial reference is all zeros. Users should
+   * call reset with the initial system state before enabling the loop.
+   *
+   * @param plant      State-space plant.
+   * @param controller State-space controller.
+   * @param observer   State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   * @param dtSeconds The nominal timestep.
+   */
+  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
+                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+                          KalmanFilter<States, Inputs, Outputs> observer,
+                          Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction,
+                          double dtSeconds) {
+    this(plant, controller, new LinearPlantInversionFeedforward<>(plant, dtSeconds),
+          observer, clampFunction);
+  }
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer. By default, the initial reference is all zeros. Users should
+   * call reset with the initial system state before enabling the loop.
+   *
+   * @param plant      State-space plant.
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer   State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the
+   *                        inputs are voltages.
+   */
+  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
+                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+                          LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+                          KalmanFilter<States, Inputs, Outputs> observer,
+                          double maxVoltageVolts
+  ) {
+    this(plant, controller, feedforward,
+          observer, u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer. By default, the initial reference is all zeros. Users should
+   * call reset with the initial system state before enabling the loop.
+   *
+   * @param plant      State-space plant.
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer   State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   */
+  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
+                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+                          LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+                          KalmanFilter<States, Inputs, Outputs> observer,
+                          Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_plant = plant;
+    this.m_controller = controller;
+    this.m_feedforward = feedforward;
+    this.m_observer = observer;
+    this.m_clampFunction = clampFunction;
+
+    m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
+    reset(m_nextR);
+  }
+
+  /**
+   * Returns the observer's state estimate x-hat.
+   *
+   * @return the observer's state estimate x-hat.
+   */
+  public Matrix<States, N1> getXHat() {
+    return getObserver().getXhat();
+  }
+
+  /**
+   * Returns an element of the observer's state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the i-th element of the observer's state estimate x-hat.
+   */
+  public double getXHat(int row) {
+    return getObserver().getXhat(row);
+  }
+
+  /**
+   * Set the initial state estimate x-hat.
+   *
+   * @param xhat The initial state estimate x-hat.
+   */
+  public void setXHat(Matrix<States, N1> xhat) {
+    getObserver().setXhat(xhat);
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row   Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXHat(int row, double value) {
+    getObserver().setXhat(row, value);
+  }
+
+  /**
+   * Returns an element of the controller's next reference r.
+   *
+   * @param row Row of r.
+   * @return the element i of the controller's next reference r.
+   */
+  public double getNextR(int row) {
+    return getNextR().get(row, 0);
+  }
+
+  /**
+   * Returns the controller's next reference r.
+   *
+   * @return the controller's next reference r.
+   */
+  public Matrix<States, N1> getNextR() {
+    return m_nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(Matrix<States, N1> nextR) {
+    m_nextR = nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(double... nextR) {
+    if (nextR.length != m_nextR.getNumRows()) {
+      throw new MatrixDimensionException(String.format("The next reference does not have the "
+                      + "correct number of entries! Expected %s, but got %s.",
+              m_nextR.getNumRows(),
+              nextR.length));
+    }
+    m_nextR = new Matrix<>(new SimpleMatrix(m_nextR.getNumRows(), 1, true, nextR));
+  }
+
+  /**
+   * Returns the controller's calculated control input u plus the calculated feedforward u_ff.
+   *
+   * @return the calculated control input u.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return clampInput(m_controller.getU().plus(m_feedforward.getUff()));
+  }
+
+  /**
+   * Returns an element of the controller's calculated control input u.
+   *
+   * @param row Row of u.
+   * @return the calculated control input u at the row i.
+   */
+  public double getU(int row) {
+    return getU().get(row, 0);
+  }
+
+  /**
+   * Return the plant used internally.
+   *
+   * @return the plant used internally.
+   */
+  public LinearSystem<States, Inputs, Outputs> getPlant() {
+    return m_plant;
+  }
+
+  /**
+   * Return the controller used internally.
+   *
+   * @return the controller used internally.
+   */
+  public LinearQuadraticRegulator<States, Inputs, Outputs> getController() {
+    return m_controller;
+  }
+
+  /**
+   * Return the feedforward used internally.
+   *
+   * @return the feedforward used internally.
+   */
+  public LinearPlantInversionFeedforward<States, Inputs, Outputs> getFeedforward() {
+    return m_feedforward;
+  }
+
+  /**
+   * Return the observer used internally.
+   *
+   * @return the observer used internally.
+   */
+  public KalmanFilter<States, Inputs, Outputs> getObserver() {
+    return m_observer;
+  }
+
+  /**
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_nextR.fill(0.0);
+    m_controller.reset();
+    m_feedforward.reset(initialState);
+    m_observer.setXhat(initialState);
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @return The state error matrix.
+   */
+  public Matrix<States, N1> getError() {
+    return getController().getR().minus(m_observer.getXhat());
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @param index The index of the error matrix to return.
+   * @return The error at that index.
+   */
+  public double getError(int index) {
+    return (getController().getR().minus(m_observer.getXhat())).get(index, 0);
+  }
+
+  /**
+   * Get the function used to clamp the input u.
+   * @return The clamping function.
+   */
+  public Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> getClampFunction() {
+    return m_clampFunction;
+  }
+
+  /**
+   * Set the clamping function used to clamp inputs.
+   */
+  public void setClampFunction(Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_clampFunction = clampFunction;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  public void correct(Matrix<Outputs, N1> y) {
+    getObserver().correct(getU(), y);
+  }
+
+  /**
+   * Sets new controller output, projects model forward, and runs observer
+   * prediction.
+   *
+   * <p>After calling this, the user should send the elements of u to the
+   * actuators.
+   *
+   * @param dtSeconds Timestep for model update.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public void predict(double dtSeconds) {
+    var u = clampInput(m_controller.calculate(getObserver().getXhat(), m_nextR)
+          .plus(m_feedforward.calculate(m_nextR)));
+    getObserver().predict(u, dtSeconds);
+  }
+
+  /**
+   * Clamp the input u to the min and max.
+   *
+   * @param unclampedU The input to clamp.
+   * @return           The clamped input.
+   */
+  public Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> unclampedU) {
+    return m_clampFunction.apply(unclampedU);
+  }
+
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java
new file mode 100644
index 0000000..a808dec
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system;
+
+import java.util.function.BiFunction;
+import java.util.function.Function;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+public final class NumericalJacobian {
+  private NumericalJacobian() {
+    // Utility Class.
+  }
+
+  private static final double kEpsilon = 1e-5;
+
+  /**
+   * Computes the numerical Jacobian with respect to x for f(x).
+   *
+   * @param <Rows>   Number of rows in the result of f(x).
+   * @param <States> Num representing the number of rows in the output of f.
+   * @param <Cols>   Number of columns in the result of f(x).
+   * @param rows     Number of rows in the result of f(x).
+   * @param cols     Number of columns in the result of f(x).
+   * @param f        Vector-valued function from which to compute the Jacobian.
+   * @param x        Vector argument.
+   * @return The numerical Jacobian with respect to x for f(x, u, ...).
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <Rows extends Num, Cols extends Num, States extends Num> Matrix<Rows, Cols>
+      numericalJacobian(
+          Nat<Rows> rows,
+          Nat<Cols> cols,
+          Function<Matrix<Cols, N1>, Matrix<States, N1>> f,
+          Matrix<Cols, N1> x
+  ) {
+    var result = new Matrix<>(rows, cols);
+
+    for (int i = 0; i < cols.getNum(); i++) {
+      var dxPlus = x.copy();
+      var dxMinus = x.copy();
+      dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon);
+      dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon);
+      @SuppressWarnings("LocalVariableName")
+      var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon);
+
+      result.setColumn(i, Matrix.changeBoundsUnchecked(dF));
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns numerical Jacobian with respect to x for f(x, u, ...).
+   *
+   * @param <Rows>    Number of rows in the result of f(x, u).
+   * @param <States>  Number of rows in x.
+   * @param <Inputs>  Number of rows in the second input to f.
+   * @param <Outputs> Num representing the rows in the output of f.
+   * @param rows      Number of rows in the result of f(x, u).
+   * @param states    Number of rows in x.
+   * @param f         Vector-valued function from which to compute Jacobian.
+   * @param x         State vector.
+   * @param u         Input vector.
+   * @return The numerical Jacobian with respect to x for f(x, u, ...).
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <Rows extends Num, States extends Num, Inputs extends Num, Outputs extends Num>
+      Matrix<Rows, States> numericalJacobianX(
+          Nat<Rows> rows,
+          Nat<States> states,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u
+  ) {
+    return numericalJacobian(rows, states, _x -> f.apply(_x, u), x);
+  }
+
+  /**
+   * Returns the numerical Jacobian with respect to u for f(x, u).
+   *
+   * @param <States> The states of the system.
+   * @param <Inputs> The inputs to the system.
+   * @param <Rows>   Number of rows in the result of f(x, u).
+   * @param rows     Number of rows in the result of f(x, u).
+   * @param inputs   Number of rows in u.
+   * @param f        Vector-valued function from which to compute the Jacobian.
+   * @param x        State vector.
+   * @param u        Input vector.
+   * @return the numerical Jacobian with respect to u for f(x, u).
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <Rows extends Num, States extends Num, Inputs extends Num> Matrix<Rows, Inputs>
+      numericalJacobianU(
+          Nat<Rows> rows,
+          Nat<Inputs> inputs,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u
+  ) {
+    return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
new file mode 100644
index 0000000..fef5ddf
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system;
+
+import java.util.function.BiFunction;
+import java.util.function.DoubleFunction;
+import java.util.function.Function;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Num;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+public final class RungeKutta {
+  private RungeKutta() {
+    // utility Class
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f         The function to integrate, which takes one argument x.
+   * @param x         The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rungeKutta(
+          DoubleFunction<Double> f,
+          double x,
+          double dtSeconds
+  ) {
+    final var halfDt = 0.5 * dtSeconds;
+    final var k1 = f.apply(x);
+    final var k2 = f.apply(x + k1 * halfDt);
+    final var k3 = f.apply(x + k2 * halfDt);
+    final var k4 = f.apply(x + k3 * dtSeconds);
+    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f         The function to integrate. It must take two arguments x and u.
+   * @param x         The initial value of x.
+   * @param u         The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return The result of Runge Kutta integration (4th order).
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rungeKutta(
+          BiFunction<Double, Double, Double> f,
+          double x, Double u, double dtSeconds
+  ) {
+    final var halfDt = 0.5 * dtSeconds;
+    final var k1 = f.apply(x, u);
+    final var k2 = f.apply(x + k1 * halfDt, u);
+    final var k3 = f.apply(x + k2 * halfDt, u);
+    final var k4 = f.apply(x + k3 * dtSeconds, u);
+    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+   *
+   * @param <States>  A Num representing the states of the system to integrate.
+   * @param <Inputs>  A Num representing the inputs of the system to integrate.
+   * @param f         The function to integrate. It must take two arguments x and u.
+   * @param x         The initial value of x.
+   * @param u         The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rungeKutta(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x, Matrix<Inputs, N1> u, double dtSeconds) {
+
+    final var halfDt = 0.5 * dtSeconds;
+    Matrix<States, N1> k1 = f.apply(x, u);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   *
+   * @param <States>  A Num prepresenting the states of the system.
+   * @param f         The function to integrate. It must take one argument x.
+   * @param x         The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num> Matrix<States, N1> rungeKutta(
+      Function<Matrix<States, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x, double dtSeconds) {
+
+    final var halfDt = 0.5 * dtSeconds;
+    Matrix<States, N1> k1 = f.apply(x);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
+  }
+
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java
new file mode 100644
index 0000000..2e95e3f
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system.plant;
+
+import edu.wpi.first.wpilibj.util.Units;
+
+/**
+ * Holds the constants for a DC motor.
+ */
+public class DCMotor {
+  public final double m_nominalVoltageVolts;
+  public final double m_stallTorqueNewtonMeters;
+  public final double m_stallCurrentAmps;
+  public final double m_freeCurrentAmps;
+  public final double m_freeSpeedRadPerSec;
+  @SuppressWarnings("MemberName")
+  public final double m_rOhms;
+  @SuppressWarnings("MemberName")
+  public final double m_KvRadPerSecPerVolt;
+  @SuppressWarnings("MemberName")
+  public final double m_KtNMPerAmp;
+
+
+  /**
+   * Constructs a DC motor.
+   *
+   * @param nominalVoltageVolts     Voltage at which the motor constants were measured.
+   * @param stallTorqueNewtonMeters Current draw when stalled.
+   * @param stallCurrentAmps        Current draw when stalled.
+   * @param freeCurrentAmps         Current draw under no load.
+   * @param freeSpeedRadPerSec      Angular velocity under no load.
+   */
+  public DCMotor(double nominalVoltageVolts,
+                 double stallTorqueNewtonMeters,
+                 double stallCurrentAmps,
+                 double freeCurrentAmps,
+                 double freeSpeedRadPerSec) {
+    this.m_nominalVoltageVolts = nominalVoltageVolts;
+    this.m_stallTorqueNewtonMeters = stallTorqueNewtonMeters;
+    this.m_stallCurrentAmps = stallCurrentAmps;
+    this.m_freeCurrentAmps = freeCurrentAmps;
+    this.m_freeSpeedRadPerSec = freeSpeedRadPerSec;
+
+    this.m_rOhms = nominalVoltageVolts / stallCurrentAmps;
+    this.m_KvRadPerSecPerVolt = freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms
+      * freeCurrentAmps);
+    this.m_KtNMPerAmp = stallTorqueNewtonMeters / stallCurrentAmps;
+  }
+
+  /**
+   * Estimate the current being drawn by this motor.
+   *
+   * @param speedRadiansPerSec The speed of the rotor.
+   * @param voltageInputVolts  The input voltage.
+   */
+  public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
+    return -1.0 / m_KvRadPerSecPerVolt / m_rOhms * speedRadiansPerSec
+      + 1.0 / m_rOhms * voltageInputVolts;
+  }
+
+  /**
+   * Return a gearbox of CIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getCIM(int numMotors) {
+    return new DCMotor(12,
+      2.42 * numMotors, 133,
+      2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310));
+  }
+
+  /**
+   * Return a gearbox of 775Pro motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getVex775Pro(int numMotors) {
+    return gearbox(new DCMotor(12,
+      0.71, 134,
+      0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of NEO motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getNEO(int numMotors) {
+    return gearbox(new DCMotor(12, 2.6,
+      105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of MiniCIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getMiniCIM(int numMotors) {
+    return gearbox(new DCMotor(12, 1.41, 89, 3,
+      Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Bag motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getBag(int numMotors) {
+    return gearbox(new DCMotor(12, 0.43, 53, 1.8,
+      Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark RS775-125 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getAndymarkRs775_125(int numMotors) {
+    return gearbox(new DCMotor(12, 0.28, 18, 1.6,
+      Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS775 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getBanebotsRs775(int numMotors) {
+    return gearbox(new DCMotor(12, 0.72, 97, 2.7,
+      Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark 9015 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getAndymark9015(int numMotors) {
+    return gearbox(new DCMotor(12, 0.36, 71, 3.7,
+      Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getBanebotsRs550(int numMotors) {
+    return gearbox(new DCMotor(12, 0.38, 84, 0.4,
+      Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Neo 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getNeo550(int numMotors) {
+    return gearbox(new DCMotor(12, 0.97, 100, 1.4,
+      Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Falcon 500 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   */
+  public static DCMotor getFalcon500(int numMotors) {
+    return gearbox(new DCMotor(12, 4.69, 257, 1.5,
+      Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors);
+  }
+
+  private static DCMotor gearbox(DCMotor motor, double numMotors) {
+    return new DCMotor(motor.m_nominalVoltageVolts, motor.m_stallTorqueNewtonMeters * numMotors,
+      motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java
new file mode 100644
index 0000000..25d1161
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java
@@ -0,0 +1,207 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system.plant;
+
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+public final class LinearSystemId {
+  private LinearSystemId() {
+    // Utility class
+  }
+
+  /**
+   * Create a state-space model of an elevator system.
+   *
+   * @param motor        The motor (or gearbox) attached to the arm.
+   * @param massKg       The mass of the elevator carriage, in kilograms.
+   * @param radiusMeters The radius of thd driving drum of the elevator, in meters.
+   * @param G            The reduction between motor and drum, as a ratio of output to input.
+   * @return A LinearSystem representing the given characterized constants.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> createElevatorSystem(DCMotor motor, double massKg,
+                                                              double radiusMeters, double G) {
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1,
+                    0, -Math.pow(G, 2) * motor.m_KtNMPerAmp
+                            / (motor.m_rOhms * radiusMeters * radiusMeters * massKg
+                            * motor.m_KvRadPerSecPerVolt)),
+            VecBuilder.fill(
+                    0, G * motor.m_KtNMPerAmp / (motor.m_rOhms * radiusMeters * massKg)),
+            Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
+            new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a flywheel system.
+   *
+   * @param motor            The motor (or gearbox) attached to the arm.
+   * @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.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N1, N1, N1> createFlywheelSystem(DCMotor motor,
+                                                              double jKgMetersSquared,
+                                                              double G) {
+    return new LinearSystem<>(
+        VecBuilder.fill(
+                    -G * G * motor.m_KtNMPerAmp
+                            / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgMetersSquared)),
+            VecBuilder.fill(G * motor.m_KtNMPerAmp
+                    / (motor.m_rOhms * jKgMetersSquared)),
+            Matrix.eye(Nat.N1()),
+            new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a differential drive drivetrain. In this model, the
+   * states are [v_left, v_right]^T, inputs are [V_left, V_right]^T and outputs are
+   * [v_left, v_right]^T.
+   *
+   * @param motor            the gearbox representing the motors driving the drivetrain.
+   * @param massKg           the mass of the robot.
+   * @param rMeters          the radius of the wheels in meters.
+   * @param rbMeters         the radius of the base (half the track width) in meters.
+   * @param JKgMetersSquared the moment of inertia of the robot.
+   * @param G                the gearing reduction as output over input.
+   * @return A LinearSystem representing a differential drivetrain.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(DCMotor motor,
+                                                                        double massKg,
+                                                                        double rMeters,
+                                                                        double rbMeters,
+                                                                        double JKgMetersSquared,
+                                                                        double G) {
+    var C1 =
+            -(G * G) * motor.m_KtNMPerAmp
+                    / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * rMeters * rMeters);
+    var C2 = G * motor.m_KtNMPerAmp / (motor.m_rOhms * rMeters);
+
+    final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
+    final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
+    var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(
+            C3 * C1,
+            C4 * C1,
+            C4 * C1,
+            C3 * C1);
+    var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(
+            C3 * C2,
+            C4 * C2,
+            C4 * C2,
+            C3 * C2);
+    var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0);
+    var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0);
+
+    return new LinearSystem<>(A, B, C, D);
+  }
+
+  /**
+   * Create a state-space model of a single jointed arm system.
+   *
+   * @param motor            The motor (or gearbox) attached to the arm.
+   * @param jKgSquaredMeters The moment of inertia J of the arm.
+   * @param G                the gearing between the motor and arm, in output over input.
+   *                         Most of the time this will be greater than 1.
+   * @return A LinearSystem representing the given characterized constants.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(DCMotor motor,
+                                                                      double jKgSquaredMeters,
+                                                                      double G) {
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1,
+                    0, -Math.pow(G, 2) * motor.m_KtNMPerAmp
+                            / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgSquaredMeters)),
+            VecBuilder.fill(0, G * motor.m_KtNMPerAmp
+                    / (motor.m_rOhms * jKgSquaredMeters)),
+            Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
+            new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2).
+   * These constants cam be found using frc-characterization.
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
+   * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
+   *
+   * @param kV         The velocity gain, in volts per (units per second)
+   * @param kA         The acceleration gain, in volts per (units per second squared)
+   * @return A LinearSystem representing the given characterized constants.
+   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
+   * https://github.com/wpilibsuite/frc-characterization</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
+    return new LinearSystem<>(
+        VecBuilder.fill(-kV / kA),
+            VecBuilder.fill(1.0 / kA),
+            VecBuilder.fill(1.0),
+            VecBuilder.fill(0.0));
+  }
+
+  /**
+   * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2).
+   * These constants cam be found using frc-characterization.
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
+   * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
+   *
+   * @param kV         The velocity gain, in volts per (units per second)
+   * @param kA         The acceleration gain, in volts per (units per second squared)
+   * @return A LinearSystem representing the given characterized constants.
+   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
+   * https://github.com/wpilibsuite/frc-characterization</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
+            VecBuilder.fill(0.0, 1.0 / kA),
+            Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0),
+            VecBuilder.fill(0.0));
+  }
+
+  /**
+   * Identify a standard differential drive drivetrain, given the drivetrain's
+   * kV and kA in both linear (volts/(meter/sec) and volts/(meter/sec^2)) and
+   * angular (volts/(radian/sec) and volts/(radian/sec^2)) cases. This can be
+   * found using frc-characterization.
+   *
+   * @param kVLinear   The linear velocity gain, volts per (meter per second).
+   * @param kALinear   The linear acceleration gain, volts per (meter per second squared).
+   * @param kVAngular  The angular velocity gain, volts per (radians per second).
+   * @param kAAngular  The angular acceleration gain, volts per (radians per second squared).
+   * @return A LinearSystem representing the given characterized constants.
+   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
+   * https://github.com/wpilibsuite/frc-characterization</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
+        double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+
+    final double c = 0.5 / (kALinear * kAAngular);
+    final double A1 = c * (-kALinear * kVAngular - kVLinear * kAAngular);
+    final double A2 = c * (kALinear * kVAngular - kVLinear * kAAngular);
+    final double B1 = c * (kALinear + kAAngular);
+    final double B2 = c * (kAAngular - kALinear);
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
+            Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
+            Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
+            Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
new file mode 100644
index 0000000..7de2d84
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
@@ -0,0 +1,349 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of
+ * various States that represent the pose, curvature, time elapsed, velocity,
+ * and acceleration at that point.
+ */
+public class Trajectory {
+  private final double m_totalTimeSeconds;
+  private final List<State> m_states;
+
+  /**
+   * Constructs an empty trajectory.
+   */
+  public Trajectory() {
+    m_states = new ArrayList<>();
+    m_totalTimeSeconds = 0.0;
+  }
+
+  /**
+   * Constructs a trajectory from a vector of states.
+   *
+   * @param states A vector of states.
+   */
+  public Trajectory(final List<State> states) {
+    m_states = states;
+    m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
+  }
+
+  /**
+   * 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.
+   */
+  @SuppressWarnings("ParameterName")
+  private static double lerp(double startValue, double endValue, double t) {
+    return startValue + (endValue - startValue) * t;
+  }
+
+  /**
+   * Linearly interpolates between two poses.
+   *
+   * @param startValue The start pose.
+   * @param endValue   The end pose.
+   * @param t          The fraction for interpolation.
+   * @return The interpolated pose.
+   */
+  @SuppressWarnings("ParameterName")
+  private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
+    return startValue.plus((endValue.minus(startValue)).times(t));
+  }
+
+  /**
+   * Returns the initial pose of the trajectory.
+   *
+   * @return The initial pose of the trajectory.
+   */
+  public Pose2d getInitialPose() {
+    return sample(0).poseMeters;
+  }
+
+  /**
+   * Returns the overall duration of the trajectory.
+   *
+   * @return The duration of the trajectory.
+   */
+  public double getTotalTimeSeconds() {
+    return m_totalTimeSeconds;
+  }
+
+  /**
+   * Return the states of the trajectory.
+   *
+   * @return The states of the trajectory.
+   */
+  public List<State> getStates() {
+    return m_states;
+  }
+
+  /**
+   * Sample the trajectory at a point in time.
+   *
+   * @param timeSeconds The point in time since the beginning of the trajectory to sample.
+   * @return The state at that point in time.
+   */
+  public State sample(double timeSeconds) {
+    if (timeSeconds <= m_states.get(0).timeSeconds) {
+      return m_states.get(0);
+    }
+    if (timeSeconds >= m_totalTimeSeconds) {
+      return m_states.get(m_states.size() - 1);
+    }
+
+    // To get the element that we want, we will use a binary search algorithm
+    // instead of iterating over a for-loop. A binary search is O(std::log(n))
+    // whereas searching using a loop is O(n).
+
+    // This starts at 1 because we use the previous state later on for
+    // interpolation.
+    int low = 1;
+    int high = m_states.size() - 1;
+
+    while (low != high) {
+      int mid = (low + high) / 2;
+      if (m_states.get(mid).timeSeconds < timeSeconds) {
+        // This index and everything under it are less than the requested
+        // timestamp. Therefore, we can discard them.
+        low = mid + 1;
+      } else {
+        // t is at least as large as the element at this index. This means that
+        // anything after it cannot be what we are looking for.
+        high = mid;
+      }
+    }
+
+    // High and Low should be the same.
+
+    // The sample's timestamp is now greater than or equal to the requested
+    // timestamp. If it is greater, we need to interpolate between the
+    // previous state and the current state to get the exact state that we
+    // want.
+    final State sample = m_states.get(low);
+    final State prevSample = m_states.get(low - 1);
+
+    // If the difference in states is negligible, then we are spot on!
+    if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
+      return sample;
+    }
+    // Interpolate between the two states for the state that we want.
+    return prevSample.interpolate(sample,
+        (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
+  }
+
+  /**
+   * Transforms all poses in the trajectory by the given transform. This is
+   * useful for converting a robot-relative trajectory into a field-relative
+   * trajectory. This works with respect to the first pose in the trajectory.
+   *
+   * @param transform The transform to transform the trajectory by.
+   * @return The transformed trajectory.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public Trajectory transformBy(Transform2d transform) {
+    var firstState = m_states.get(0);
+    var firstPose = firstState.poseMeters;
+
+    // Calculate the transformed first pose.
+    var newFirstPose = firstPose.plus(transform);
+    List<State> newStates = new ArrayList<>();
+
+    newStates.add(new State(
+        firstState.timeSeconds, firstState.velocityMetersPerSecond,
+        firstState.accelerationMetersPerSecondSq, newFirstPose, firstState.curvatureRadPerMeter
+    ));
+
+    for (int i = 1; i < m_states.size(); i++) {
+      var state = m_states.get(i);
+      // We are transforming relative to the coordinate frame of the new initial pose.
+      newStates.add(new State(
+          state.timeSeconds, state.velocityMetersPerSecond,
+          state.accelerationMetersPerSecondSq, newFirstPose.plus(state.poseMeters.minus(firstPose)),
+          state.curvatureRadPerMeter
+      ));
+    }
+
+    return new Trajectory(newStates);
+  }
+
+  /**
+   * Transforms all poses in the trajectory so that they are relative to the
+   * given pose. This is useful for converting a field-relative trajectory
+   * into a robot-relative trajectory.
+   *
+   * @param pose The pose that is the origin of the coordinate frame that
+   *             the current trajectory will be transformed into.
+   * @return The transformed trajectory.
+   */
+  public Trajectory relativeTo(Pose2d pose) {
+    return new Trajectory(m_states.stream().map(state -> new State(state.timeSeconds,
+        state.velocityMetersPerSecond, state.accelerationMetersPerSecondSq,
+        state.poseMeters.relativeTo(pose), state.curvatureRadPerMeter))
+        .collect(Collectors.toList()));
+  }
+
+  /**
+   * Represents a time-parameterized trajectory. The trajectory contains of
+   * various States that represent the pose, curvature, time elapsed, velocity,
+   * and acceleration at that point.
+   */
+  @SuppressWarnings("MemberName")
+  public static class State {
+    // The time elapsed since the beginning of the trajectory.
+    @JsonProperty("time")
+    public double timeSeconds;
+
+    // The speed at that point of the trajectory.
+    @JsonProperty("velocity")
+    public double velocityMetersPerSecond;
+
+    // The acceleration at that point of the trajectory.
+    @JsonProperty("acceleration")
+    public double accelerationMetersPerSecondSq;
+
+    // The pose at that point of the trajectory.
+    @JsonProperty("pose")
+    public Pose2d poseMeters;
+
+    // The curvature at that point of the trajectory.
+    @JsonProperty("curvature")
+    public double curvatureRadPerMeter;
+
+    public State() {
+      poseMeters = new Pose2d();
+    }
+
+    /**
+     * Constructs a State with the specified parameters.
+     *
+     * @param timeSeconds                   The time elapsed since the beginning of the trajectory.
+     * @param velocityMetersPerSecond       The speed at that point of the trajectory.
+     * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
+     * @param poseMeters                    The pose at that point of the trajectory.
+     * @param curvatureRadPerMeter          The curvature at that point of the trajectory.
+     */
+    public State(double timeSeconds, double velocityMetersPerSecond,
+                 double accelerationMetersPerSecondSq, Pose2d poseMeters,
+                 double curvatureRadPerMeter) {
+      this.timeSeconds = timeSeconds;
+      this.velocityMetersPerSecond = velocityMetersPerSecond;
+      this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
+      this.poseMeters = poseMeters;
+      this.curvatureRadPerMeter = curvatureRadPerMeter;
+    }
+
+    /**
+     * Interpolates between two States.
+     *
+     * @param endValue The end value for the interpolation.
+     * @param i        The interpolant (fraction).
+     * @return The interpolated state.
+     */
+    @SuppressWarnings("ParameterName")
+    State interpolate(State endValue, double i) {
+      // Find the new t value.
+      final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
+
+      // Find the delta time between the current state and the interpolated state.
+      final double deltaT = newT - timeSeconds;
+
+      // If delta time is negative, flip the order of interpolation.
+      if (deltaT < 0) {
+        return endValue.interpolate(this, 1 - i);
+      }
+
+      // Check whether the robot is reversing at this stage.
+      final boolean reversing = velocityMetersPerSecond < 0
+          || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
+
+      // Calculate the new velocity
+      // v_f = v_0 + at
+      final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
+
+      // Calculate the change in position.
+      // delta_s = v_0 t + 0.5 at^2
+      final double newS = (velocityMetersPerSecond * deltaT
+          + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0);
+
+      // Return the new state. To find the new position for the new state, we need
+      // to interpolate between the two endpoint poses. The fraction for
+      // interpolation is the change in position (delta s) divided by the total
+      // distance between the two endpoints.
+      final double interpolationFrac = newS
+          / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
+
+      return new State(
+          newT, newV, accelerationMetersPerSecondSq,
+          lerp(poseMeters, endValue.poseMeters, interpolationFrac),
+          lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
+      );
+    }
+
+    @Override
+    public String toString() {
+      return String.format(
+        "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
+        timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
+        poseMeters, curvatureRadPerMeter);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (this == obj) {
+        return true;
+      }
+      if (!(obj instanceof State)) {
+        return false;
+      }
+      State state = (State) obj;
+      return Double.compare(state.timeSeconds, timeSeconds) == 0
+              && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
+              && Double.compare(state.accelerationMetersPerSecondSq,
+                accelerationMetersPerSecondSq) == 0
+              && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
+              && Objects.equals(poseMeters, state.poseMeters);
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(timeSeconds, velocityMetersPerSecond,
+              accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
+    }
+  }
+
+  @Override
+  public String toString() {
+    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
+    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
+  }
+
+  @Override
+  public int hashCode() {
+    return m_states.hashCode();
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Trajectory && m_states.equals(((Trajectory) obj).getStates());
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
new file mode 100644
index 0000000..6c9b56a
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+/**
+ * Represents the configuration for generating a trajectory. This class stores the start velocity,
+ * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
+ *
+ * <p>The class must be constructed with a max velocity and max acceleration. The other parameters
+ * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable
+ * values (0, 0, {}, false). These values can be changed via the setXXX methods.
+ */
+public class TrajectoryConfig {
+  private final double m_maxVelocity;
+  private final double m_maxAcceleration;
+  private final List<TrajectoryConstraint> m_constraints;
+  private double m_startVelocity;
+  private double m_endVelocity;
+  private boolean m_reversed;
+
+  /**
+   * Constructs the trajectory configuration class.
+   *
+   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   */
+  public TrajectoryConfig(double maxVelocityMetersPerSecond,
+                          double maxAccelerationMetersPerSecondSq) {
+    m_maxVelocity = maxVelocityMetersPerSecond;
+    m_maxAcceleration = maxAccelerationMetersPerSecondSq;
+    m_constraints = new ArrayList<>();
+  }
+
+  /**
+   * Adds a user-defined constraint to the trajectory.
+   *
+   * @param constraint The user-defined constraint.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
+    m_constraints.add(constraint);
+    return this;
+  }
+
+  /**
+   * Adds all user-defined constraints from a list to the trajectory.
+   * @param constraints List of user-defined constraints.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
+    m_constraints.addAll(constraints);
+    return this;
+  }
+
+  /**
+   * Adds a differential drive kinematics constraint to ensure that
+   * no wheel velocity of a differential drive goes above the max velocity.
+   *
+   * @param kinematics The differential drive kinematics.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
+    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+  * Adds a mecanum drive kinematics constraint to ensure that
+  * no wheel velocity of a mecanum drive goes above the max velocity.
+  *
+  * @param kinematics The mecanum drive kinematics.
+  * @return Instance of the current config object.
+  */
+  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
+    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+  * Adds a swerve drive kinematics constraint to ensure that
+  * no wheel velocity of a swerve drive goes above the max velocity.
+  *
+  * @param kinematics The swerve drive kinematics.
+  * @return Instance of the current config object.
+  */
+  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
+    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+  * Returns the starting velocity of the trajectory.
+  *
+  * @return The starting velocity of the trajectory.
+  */
+  public double getStartVelocity() {
+    return m_startVelocity;
+  }
+
+  /**
+   * Sets the start velocity of the trajectory.
+   *
+   * @param startVelocityMetersPerSecond The start velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
+    m_startVelocity = startVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   *
+   * @return The starting velocity of the trajectory.
+   */
+  public double getEndVelocity() {
+    return m_endVelocity;
+  }
+
+  /**
+   * Sets the end velocity of the trajectory.
+   *
+   * @param endVelocityMetersPerSecond The end velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
+    m_endVelocity = endVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the maximum velocity of the trajectory.
+   *
+   * @return The maximum velocity of the trajectory.
+   */
+  public double getMaxVelocity() {
+    return m_maxVelocity;
+  }
+
+  /**
+   * Returns the maximum acceleration of the trajectory.
+   *
+   * @return The maximum acceleration of the trajectory.
+   */
+  public double getMaxAcceleration() {
+    return m_maxAcceleration;
+  }
+
+  /**
+   * Returns the user-defined constraints of the trajectory.
+   *
+   * @return The user-defined constraints of the trajectory.
+   */
+  public List<TrajectoryConstraint> getConstraints() {
+    return m_constraints;
+  }
+
+  /**
+   * Returns whether the trajectory is reversed or not.
+   *
+   * @return whether the trajectory is reversed or not.
+   */
+  public boolean isReversed() {
+    return m_reversed;
+  }
+
+  /**
+   * Sets the reversed flag of the trajectory.
+   *
+   * @param reversed Whether the trajectory should be reversed or not.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setReversed(boolean reversed) {
+    m_reversed = reversed;
+    return this;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
new file mode 100644
index 0000000..5e55c50
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
@@ -0,0 +1,278 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.List;
+import java.util.function.BiConsumer;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
+import edu.wpi.first.wpilibj.spline.Spline;
+import edu.wpi.first.wpilibj.spline.SplineHelper;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+public final class TrajectoryGenerator {
+  private static final Trajectory kDoNothingTrajectory =
+      new Trajectory(Arrays.asList(new Trajectory.State()));
+  private static BiConsumer<String, StackTraceElement[]> errorFunc;
+
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private TrajectoryGenerator() {
+  }
+
+  private static void reportError(String error, StackTraceElement[] stackTrace) {
+    if (errorFunc != null) {
+      errorFunc.accept(error, stackTrace);
+    } else {
+      MathSharedStore.reportError(error, stackTrace);
+    }
+  }
+
+  /**
+   * Set error reporting function. By default, DriverStation.reportError() is used.
+   *
+   * @param func Error reporting function, arguments are error and stackTrace.
+   */
+  public static void setErrorHandler(BiConsumer<String, StackTraceElement[]> func) {
+    errorFunc = func;
+  }
+
+  /**
+   * Generates a trajectory from the given control vectors and config. This method uses clamped
+   * cubic splines -- a method in which the exterior control vectors and interior waypoints
+   * are provided. The headings are automatically determined at the interior points to
+   * ensure continuous curvature.
+   *
+   * @param initial           The initial control vector.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending control vector.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  public static Trajectory generateTrajectory(
+      Spline.ControlVector initial,
+      List<Translation2d> interiorWaypoints,
+      Spline.ControlVector end,
+      TrajectoryConfig config
+  ) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    // Clone the control vectors.
+    var newInitial = new Spline.ControlVector(initial.x, initial.y);
+    var newEnd = new Spline.ControlVector(end.x, end.y);
+
+    // Change the orientation if reversed.
+    if (config.isReversed()) {
+      newInitial.x[1] *= -1;
+      newInitial.y[1] *= -1;
+      newEnd.x[1] *= -1;
+      newEnd.y[1] *= -1;
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
+          interiorWaypoints.toArray(new Translation2d[0]), newEnd));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+        config.getMaxAcceleration(), config.isReversed());
+  }
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method uses clamped
+   * cubic splines -- a method in which the initial pose, final pose, and interior waypoints
+   * are provided.  The headings are automatically determined at the interior points to
+   * ensure continuous curvature.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  public static Trajectory generateTrajectory(
+      Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end,
+      TrajectoryConfig config
+  ) {
+    var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(
+        start, interiorWaypoints.toArray(new Translation2d[0]), end
+    );
+
+    // Return the generated trajectory.
+    return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
+  }
+
+  /**
+   * Generates a trajectory from the given quintic control vectors and config. This method
+   * uses quintic hermite splines -- therefore, all points must be represented by control
+   * vectors. Continuous curvature is guaranteed in this method.
+   *
+   * @param controlVectors List of quintic control vectors.
+   * @param config         The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static Trajectory generateTrajectory(
+      ControlVectorList controlVectors,
+      TrajectoryConfig config
+  ) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+    final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
+
+    // Create a new control vector list, flipping the orientation if reversed.
+    for (final var vector : controlVectors) {
+      var newVector = new Spline.ControlVector(vector.x, vector.y);
+      if (config.isReversed()) {
+        newVector.x[1] *= -1;
+        newVector.y[1] *= -1;
+      }
+      newControlVectors.add(newVector);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
+          newControlVectors.toArray(new Spline.ControlVector[]{})
+      ));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+        config.getMaxAcceleration(), config.isReversed());
+
+  }
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method
+   * uses quintic hermite splines -- therefore, all points must be represented by Pose2d
+   * objects. Continuous curvature is guaranteed in this method.
+   *
+   * @param waypoints List of waypoints..
+   * @param config    The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    List<Pose2d> newWaypoints = new ArrayList<>();
+    if (config.isReversed()) {
+      for (Pose2d originalWaypoint : waypoints) {
+        newWaypoints.add(originalWaypoint.plus(flip));
+      }
+    } else {
+      newWaypoints.addAll(waypoints);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+        config.getMaxAcceleration(), config.isReversed());
+  }
+
+  /**
+   * Generate spline points from a vector of splines by parameterizing the
+   * splines.
+   *
+   * @param splines The splines to parameterize.
+   * @return The spline points for use in time parameterization of a trajectory.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *                                  with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> splinePointsFromSplines(
+      Spline[] splines) {
+    // Create the vector of spline points.
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // Add the first point to the vector.
+    splinePoints.add(splines[0].getPoint(0.0));
+
+    // Iterate through the vector and parameterize each spline, adding the
+    // parameterized points to the final vector.
+    for (final var spline : splines) {
+      var points = SplineParameterizer.parameterize(spline);
+
+      // Append the array of poses to the vector. We are removing the first
+      // point because it's a duplicate of the last point from the previous
+      // spline.
+      splinePoints.addAll(points.subList(1, points.size()));
+    }
+    return splinePoints;
+  }
+
+  // Work around type erasure signatures
+  @SuppressWarnings("serial")
+  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
+    public ControlVectorList(int initialCapacity) {
+      super(initialCapacity);
+    }
+
+    public ControlVectorList() {
+      super();
+    }
+
+    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
+      super(collection);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
new file mode 100644
index 0000000..3b1d2af
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
@@ -0,0 +1,318 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+/**
+ * Class used to parameterize a trajectory by time.
+ */
+public final class TrajectoryParameterizer {
+  /**
+   * Private constructor because this is a utility class.
+   */
+  private TrajectoryParameterizer() {
+  }
+
+  /**
+   * Parameterize the trajectory by time. This is where the velocity profile is
+   * generated.
+   *
+   * <p>The derivation of the algorithm used can be found
+   * <a href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">
+   * here</a>.
+   *
+   * @param points                           Reference to the spline points.
+   * @param constraints                      A vector of various velocity and acceleration.
+   *                                         constraints.
+   * @param startVelocityMetersPerSecond     The start velocity for the trajectory.
+   * @param endVelocityMetersPerSecond       The end velocity for the trajectory.
+   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   * @param reversed                         Whether the robot should move backwards.
+   *                                         Note that the robot will still move from
+   *                                         a -&gt; b -&gt; ... -&gt; z as defined in the
+   *                                         waypoints.
+   * @return The trajectory.
+   */
+  @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
+      "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public static Trajectory timeParameterizeTrajectory(
+      List<PoseWithCurvature> points,
+      List<TrajectoryConstraint> constraints,
+      double startVelocityMetersPerSecond,
+      double endVelocityMetersPerSecond,
+      double maxVelocityMetersPerSecond,
+      double maxAccelerationMetersPerSecondSq,
+      boolean reversed
+  ) {
+    var constrainedStates = new ArrayList<ConstrainedState>(points.size());
+    var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond,
+        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
+
+    // Forward pass
+    for (int i = 0; i < points.size(); i++) {
+      constrainedStates.add(new ConstrainedState());
+      var constrainedState = constrainedStates.get(i);
+      constrainedState.pose = points.get(i);
+
+      // Begin constraining based on predecessor.
+      double ds = constrainedState.pose.poseMeters.getTranslation().getDistance(
+          predecessor.pose.poseMeters.getTranslation());
+      constrainedState.distanceMeters = predecessor.distanceMeters + ds;
+
+      // We may need to iterate to find the maximum end velocity and common
+      // acceleration, since acceleration limits may be a function of velocity.
+      while (true) {
+        // Enforce global max velocity and max reachable velocity by global
+        // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+        constrainedState.maxVelocityMetersPerSecond = Math.min(
+            maxVelocityMetersPerSecond,
+            Math.sqrt(predecessor.maxVelocityMetersPerSecond
+                * predecessor.maxVelocityMetersPerSecond
+                + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)
+        );
+
+        constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
+        constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+
+        // At this point, the constrained state is fully constructed apart from
+        // all the custom-defined user constraints.
+        for (final var constraint : constraints) {
+          constrainedState.maxVelocityMetersPerSecond = Math.min(
+              constrainedState.maxVelocityMetersPerSecond,
+              constraint.getMaxVelocityMetersPerSecond(
+                  constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter,
+                  constrainedState.maxVelocityMetersPerSecond)
+          );
+        }
+
+        // Now enforce all acceleration limits.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds < 1E-6) {
+          break;
+        }
+
+        // If the actual acceleration for this state is higher than the max
+        // acceleration that we applied, then we need to reduce the max
+        // acceleration of the predecessor and try again.
+        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
+            * constrainedState.maxVelocityMetersPerSecond
+            - predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond)
+            / (ds * 2.0);
+
+        // If we violate the max acceleration constraint, let's modify the
+        // predecessor.
+        if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
+          predecessor.maxAccelerationMetersPerSecondSq
+              = constrainedState.maxAccelerationMetersPerSecondSq;
+        } else {
+          // Constrain the predecessor's max acceleration to the current
+          // acceleration.
+          if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
+            predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
+          }
+          // If the actual acceleration is less than the predecessor's min
+          // acceleration, it will be repaired in the backward pass.
+          break;
+        }
+      }
+      predecessor = constrainedState;
+    }
+
+    var successor = new ConstrainedState(points.get(points.size() - 1),
+        constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
+        endVelocityMetersPerSecond,
+        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
+
+    // Backward pass
+    for (int i = points.size() - 1; i >= 0; i--) {
+      var constrainedState = constrainedStates.get(i);
+      double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
+
+      while (true) {
+        // Enforce max velocity limit (reverse)
+        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+        double newMaxVelocity = Math.sqrt(
+            successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+                + successor.minAccelerationMetersPerSecondSq * ds * 2.0
+        );
+
+        // No more limits to impose! This state can be finalized.
+        if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
+          break;
+        }
+
+        constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
+
+        // Check all acceleration constraints with the new max velocity.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds > -1E-6) {
+          break;
+        }
+
+        // If the actual acceleration for this state is lower than the min
+        // acceleration, then we need to lower the min acceleration of the
+        // successor and try again.
+        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
+            * constrainedState.maxVelocityMetersPerSecond
+            - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
+            / (ds * 2.0);
+
+        if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
+          successor.minAccelerationMetersPerSecondSq
+              = constrainedState.minAccelerationMetersPerSecondSq;
+        } else {
+          successor.minAccelerationMetersPerSecondSq = actualAcceleration;
+          break;
+        }
+      }
+      successor = constrainedState;
+    }
+
+    // Now we can integrate the constrained states forward in time to obtain our
+    // trajectory states.
+    var states = new ArrayList<Trajectory.State>(points.size());
+    double timeSeconds = 0.0;
+    double distanceMeters = 0.0;
+    double velocityMetersPerSecond = 0.0;
+
+    for (int i = 0; i < constrainedStates.size(); i++) {
+      final var state = constrainedStates.get(i);
+
+      // Calculate the change in position between the current state and the previous
+      // state.
+      double ds = state.distanceMeters - distanceMeters;
+
+      // Calculate the acceleration between the current state and the previous
+      // state.
+      double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
+          - velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2);
+
+      // Calculate dt
+      double dt = 0.0;
+      if (i > 0) {
+        states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
+        if (Math.abs(accel) > 1E-6) {
+          // v_f = v_0 + a * t
+          dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
+        } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
+          // delta_x = v * t
+          dt = ds / velocityMetersPerSecond;
+        } else {
+          throw new TrajectoryGenerationException("Something went wrong at iteration " + i
+              + " of time parameterization.");
+        }
+      }
+
+      velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
+      distanceMeters = state.distanceMeters;
+
+      timeSeconds += dt;
+
+      states.add(new Trajectory.State(
+          timeSeconds,
+          reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
+          reversed ? -accel : accel,
+          state.pose.poseMeters, state.pose.curvatureRadPerMeter
+      ));
+    }
+
+    return new Trajectory(states);
+  }
+
+  private static void enforceAccelerationLimits(boolean reverse,
+                                                List<TrajectoryConstraint> constraints,
+                                                ConstrainedState state) {
+
+    for (final var constraint : constraints) {
+      double factor = reverse ? -1.0 : 1.0;
+      final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq(
+          state.pose.poseMeters, state.pose.curvatureRadPerMeter,
+          state.maxVelocityMetersPerSecond * factor);
+
+      if (minMaxAccel.minAccelerationMetersPerSecondSq
+          > minMaxAccel.maxAccelerationMetersPerSecondSq) {
+        throw new TrajectoryGenerationException("The constraint's min acceleration "
+            + "was greater than its max acceleration.\n Offending Constraint: "
+            + constraint.getClass().getName()
+            + "\n If the offending constraint was packaged with WPILib, please file a bug report.");
+      }
+
+      state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
+          reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
+              : minMaxAccel.minAccelerationMetersPerSecondSq);
+
+      state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq,
+          reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq
+              : minMaxAccel.maxAccelerationMetersPerSecondSq);
+    }
+
+  }
+
+  @SuppressWarnings("MemberName")
+  private static class ConstrainedState {
+    PoseWithCurvature pose;
+    double distanceMeters;
+    double maxVelocityMetersPerSecond;
+    double minAccelerationMetersPerSecondSq;
+    double maxAccelerationMetersPerSecondSq;
+
+    ConstrainedState(PoseWithCurvature pose, double distanceMeters,
+                     double maxVelocityMetersPerSecond,
+                     double minAccelerationMetersPerSecondSq,
+                     double maxAccelerationMetersPerSecondSq) {
+      this.pose = pose;
+      this.distanceMeters = distanceMeters;
+      this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    ConstrainedState() {
+      pose = new PoseWithCurvature();
+    }
+  }
+
+  @SuppressWarnings("serial")
+  public static class TrajectoryGenerationException extends RuntimeException {
+    public TrajectoryGenerationException(String message) {
+      super(message);
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
similarity index 100%
rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
rename to wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
new file mode 100644
index 0000000..8289d4d
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
@@ -0,0 +1,304 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Objects;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * <p>While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on trapezoidal
+ * velocity constraints. To compute the reference obeying this constraint, do
+ * the following.
+ *
+ * <p>Initialization:
+ * <pre><code>
+ * TrapezoidProfile.Constraints constraints =
+ *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
+ * TrapezoidProfile.State previousProfiledReference =
+ *   new TrapezoidProfile.State(initialReference, 0.0);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ * <pre><code>
+ * TrapezoidProfile profile =
+ *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
+ * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for
+ * `calculate()` and to determine when the profile has completed via
+ * `isFinished()`.
+ */
+public class TrapezoidProfile {
+  // The direction of the profile, either 1 for forwards or -1 for inverted
+  private int m_direction;
+
+  private Constraints m_constraints;
+  private State m_initial;
+  private State m_goal;
+
+  private double m_endAccel;
+  private double m_endFullSpeed;
+  private double m_endDeccel;
+
+  public static class Constraints {
+    @SuppressWarnings("MemberName")
+    public double maxVelocity;
+    @SuppressWarnings("MemberName")
+    public double maxAcceleration;
+
+    public Constraints() {
+      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
+    }
+
+    /**
+     * Construct constraints for a TrapezoidProfile.
+     *
+     * @param maxVelocity maximum velocity
+     * @param maxAcceleration maximum acceleration
+     */
+    public Constraints(double maxVelocity, double maxAcceleration) {
+      this.maxVelocity = maxVelocity;
+      this.maxAcceleration = maxAcceleration;
+      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
+    }
+  }
+
+  public static class State {
+    @SuppressWarnings("MemberName")
+    public double position;
+    @SuppressWarnings("MemberName")
+    public double velocity;
+
+    public State() {
+    }
+
+    public State(double position, double velocity) {
+      this.position = position;
+      this.velocity = velocity;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+      if (other instanceof State) {
+        State rhs = (State) other;
+        return this.position == rhs.position && this.velocity == rhs.velocity;
+      } else {
+        return false;
+      }
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(position, velocity);
+    }
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   * @param initial     The initial state (usually the current state).
+   */
+  public TrapezoidProfile(Constraints constraints, State goal, State initial) {
+    m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
+    m_constraints = constraints;
+    m_initial = direct(initial);
+    m_goal = direct(goal);
+
+    if (m_initial.velocity > m_constraints.maxVelocity) {
+      m_initial.velocity = m_constraints.maxVelocity;
+    }
+
+    // Deal with a possibly truncated motion profile (with nonzero initial or
+    // final velocity) by calculating the parameters as if the profile began and
+    // ended at zero velocity
+    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+    // Now we can calculate the parameters as if it was a full trapezoid instead
+    // of a truncated one
+
+    double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
+        + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
+        * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   */
+  public TrapezoidProfile(Constraints constraints, State goal) {
+    this(constraints, goal, new State(0, 0));
+  }
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  @SuppressWarnings("ParameterName")
+  public State calculate(double t) {
+    State result = new State(m_initial.position, m_initial.velocity);
+
+    if (t < m_endAccel) {
+      result.velocity += t * m_constraints.maxAcceleration;
+      result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+    } else if (t < m_endFullSpeed) {
+      result.velocity = m_constraints.maxVelocity;
+      result.position += (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration
+          / 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel);
+    } else if (t <= m_endDeccel) {
+      result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+      double timeLeft = m_endDeccel - t;
+      result.position = m_goal.position - (m_goal.velocity + timeLeft
+          * m_constraints.maxAcceleration / 2.0) * timeLeft;
+    } else {
+      result = m_goal;
+    }
+
+    return direct(result);
+  }
+
+  /**
+   * Returns the time left until a target distance in the profile is reached.
+   *
+   * @param target The target distance.
+   */
+  public double timeLeftUntil(double target) {
+    double position = m_initial.position * m_direction;
+    double velocity = m_initial.velocity * m_direction;
+
+    double endAccel = m_endAccel * m_direction;
+    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+    if (target < position) {
+      endAccel = -endAccel;
+      endFullSpeed = -endFullSpeed;
+      velocity = -velocity;
+    }
+
+    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;
+
+    double distToTarget = Math.abs(target - position);
+    if (distToTarget < 1e-6) {
+      return 0;
+    }
+
+    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+    double deccelVelocity;
+    if (endAccel > 0) {
+      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
+    } else {
+      deccelVelocity = velocity;
+    }
+
+    double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+    deccelDist = Math.max(deccelDist, 0);
+
+    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+    if (accelDist > distToTarget) {
+      accelDist = distToTarget;
+      fullSpeedDist = 0;
+      deccelDist = 0;
+    } else if (accelDist + fullSpeedDist > distToTarget) {
+      fullSpeedDist = distToTarget - accelDist;
+      deccelDist = 0;
+    } else {
+      deccelDist = distToTarget - fullSpeedDist - accelDist;
+    }
+
+    double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
+        * accelDist))) / acceleration;
+
+    double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
+        + 2 * decceleration * deccelDist))) / decceleration;
+
+    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+    return accelTime + fullSpeedTime + deccelTime;
+  }
+
+  /**
+   * Returns the total time the profile takes to reach the goal.
+   */
+  public double totalTime() {
+    return m_endDeccel;
+  }
+
+  /**
+   * Returns true if the profile has reached the goal.
+   *
+   * <p>The profile has reached the goal if the time since the profile started
+   * has exceeded the profile's total time.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  @SuppressWarnings("ParameterName")
+  public boolean isFinished(double t) {
+    return t >= totalTime();
+  }
+
+  /**
+   * Returns true if the profile inverted.
+   *
+   * <p>The profile is inverted if goal position is less than the initial position.
+   *
+   * @param initial     The initial state (usually the current state).
+   * @param goal        The desired state when the profile is complete.
+   */
+  private static boolean shouldFlipAcceleration(State initial, State goal) {
+    return initial.position > goal.position;
+  }
+
+  // Flip the sign of the velocity and position if the profile is inverted
+  private State direct(State in) {
+    State result = new State(in.position, in.velocity);
+    result.position = result.position * m_direction;
+    result.velocity = result.velocity * m_direction;
+    return result;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
new file mode 100644
index 0000000..0b87a64
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * A constraint on the maximum absolute centripetal acceleration allowed when
+ * traversing a trajectory. The centripetal acceleration of a robot is defined
+ * as the velocity squared divided by the radius of curvature.
+ *
+ * <p>Effectively, limiting the maximum centripetal acceleration will cause the
+ * robot to slow down around tight turns, making it easier to track trajectories
+ * with sharp turns.
+ */
+public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
+  private final double m_maxCentripetalAccelerationMetersPerSecondSq;
+
+  /**
+   * Constructs a centripetal acceleration constraint.
+   *
+   * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
+   */
+  public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
+    m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // ac = v^2 / r
+    // k (curvature) = 1 / r
+
+    // therefore, ac = v^2 * k
+    // ac / k = v^2
+    // v = std::sqrt(ac / k)
+
+    return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq
+        / Math.abs(curvatureRadPerMeter));
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    // The acceleration of the robot has no impact on the centripetal acceleration
+    // of the robot.
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
new file mode 100644
index 0000000..67cddcf
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final DifferentialDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a differential drive dynamics constraint.
+   *
+   * @param kinematics A kinematics component describing the drive geometry.
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics,
+                                               double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond,
+        0, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Return the new linear chassis speed.
+    return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
new file mode 100644
index 0000000..9e28b0c
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
+import static edu.wpi.first.wpiutil.ErrorMessages.requireNonNullParam;
+
+/**
+ * A class that enforces constraints on differential drive voltage expenditure based on the motor
+ * dynamics and the drive kinematics.  Ensures that the acceleration of any wheel of the robot
+ * while following the trajectory is never higher than what can be achieved with the given
+ * maximum voltage.
+ */
+public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
+  private final SimpleMotorFeedforward m_feedforward;
+  private final DifferentialDriveKinematics m_kinematics;
+  private final double m_maxVoltage;
+
+  /**
+   * Creates a new DifferentialDriveVoltageConstraint.
+   *
+   * @param feedforward A feedforward component describing the behavior of the drive.
+   * @param kinematics  A kinematics component describing the drive geometry.
+   * @param maxVoltage  The maximum voltage available to the motors while following the path.
+   *                    Should be somewhat less than the nominal battery voltage (12V) to account
+   *                    for "voltage sag" due to current draw.
+   */
+  public DifferentialDriveVoltageConstraint(SimpleMotorFeedforward feedforward,
+                                            DifferentialDriveKinematics kinematics,
+                                            double maxVoltage) {
+    m_feedforward = requireNonNullParam(feedforward, "feedforward",
+                                        "DifferentialDriveVoltageConstraint");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics",
+                                       "DifferentialDriveVoltageConstraint");
+    m_maxVoltage = maxVoltage;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    return Double.POSITIVE_INFINITY;
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocityMetersPerSecond, 0,
+                                                                   velocityMetersPerSecond
+                                                                       * curvatureRadPerMeter));
+
+    double maxWheelSpeed = Math.max(wheelSpeeds.leftMetersPerSecond,
+                                    wheelSpeeds.rightMetersPerSecond);
+    double minWheelSpeed = Math.min(wheelSpeeds.leftMetersPerSecond,
+                                    wheelSpeeds.rightMetersPerSecond);
+
+    // Calculate maximum/minimum possible accelerations from motor dynamics
+    // and max/min wheel speeds
+    double maxWheelAcceleration =
+        m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+    double minWheelAcceleration =
+        m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+    // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
+    // increased by half of the trackwidth T.  Inner wheel has radius decreased
+    // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
+    // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2).
+    // Inner wheel is similar.
+
+    // sgn(speed) term added to correctly account for which wheel is on
+    // outside of turn:
+    // If moving forward, max acceleration constraint corresponds to wheel on outside of turn
+    // If moving backward, max acceleration constraint corresponds to wheel on inside of turn
+
+    // When velocity is zero, then wheel velocities are uniformly zero (robot cannot be
+    // turning on its center) - we have to treat this as a special case, as it breaks
+    // the signum function.  Both max and min acceleration are *reduced in magnitude*
+    // in this case.
+
+    double maxChassisAcceleration;
+    double minChassisAcceleration;
+
+    if (velocityMetersPerSecond == 0) {
+      maxChassisAcceleration =
+          maxWheelAcceleration
+              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
+      minChassisAcceleration =
+          minWheelAcceleration
+              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
+    } else {
+      maxChassisAcceleration =
+          maxWheelAcceleration
+              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
+              * Math.signum(velocityMetersPerSecond) / 2);
+      minChassisAcceleration =
+          minWheelAcceleration
+              / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
+              * Math.signum(velocityMetersPerSecond) / 2);
+    }
+
+    // When turning about a point inside of the wheelbase (i.e. radius less than half
+    // the trackwidth), the inner wheel's direction changes, but the magnitude remains
+    // the same.  The formula above changes sign for the inner wheel when this happens.
+    // We can accurately account for this by simply negating the inner wheel.
+
+    if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
+      if (velocityMetersPerSecond > 0) {
+        minChassisAcceleration = -minChassisAcceleration;
+      } else if (velocityMetersPerSecond < 0) {
+        maxChassisAcceleration = -maxChassisAcceleration;
+      }
+    }
+
+    return new MinMax(minChassisAcceleration, maxChassisAcceleration);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
new file mode 100644
index 0000000..eb9f7e7
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Enforces a particular constraint only within an elliptical region.
+ */
+public class EllipticalRegionConstraint implements TrajectoryConstraint {
+  private final Translation2d m_center;
+  private final Translation2d m_radii;
+  private final TrajectoryConstraint m_constraint;
+
+  /**
+   * Constructs a new EllipticalRegionConstraint.
+   *
+   * @param center     The center of the ellipse in which to enforce the constraint.
+   * @param xWidth     The width of the ellipse in which to enforce the constraint.
+   * @param yWidth     The height of the ellipse in which to enforce the constraint.
+   * @param rotation   The rotation to apply to all radii around the origin.
+   * @param constraint The constraint to enforce when the robot is within the region.
+   */
+  @SuppressWarnings("ParameterName")
+  public EllipticalRegionConstraint(Translation2d center, double xWidth, double yWidth,
+                                    Rotation2d rotation, TrajectoryConstraint constraint) {
+    m_center = center;
+    m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation);
+    m_constraint = constraint;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter,
+          velocityMetersPerSecond);
+    } else {
+      return Double.POSITIVE_INFINITY;
+    }
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters,
+          curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return new MinMax();
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the constraint
+   * is enforced in.
+   *
+   * @param robotPose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  public boolean isPoseInRegion(Pose2d robotPose) {
+    // The region (disk) bounded by the ellipse is given by the equation:
+    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+    // If the inequality is satisfied, then it is inside the ellipse; otherwise
+    // it is outside the ellipse.
+    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
+    return Math.pow(robotPose.getX() - m_center.getX(), 2)
+        * Math.pow(m_radii.getY(), 2)
+        + Math.pow(robotPose.getY() - m_center.getY(), 2)
+        * Math.pow(m_radii.getX(), 2) <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java
new file mode 100644
index 0000000..4d60623
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * Represents a constraint that enforces a max velocity. This can be composed with the
+ * {@link EllipticalRegionConstraint} or {@link RectangularRegionConstraint} to enforce
+ * a max velocity in a region.
+ */
+public class MaxVelocityConstraint implements TrajectoryConstraint {
+  private final double m_maxVelocity;
+
+  /**
+   * Constructs a new MaxVelocityConstraint.
+   *
+   * @param maxVelocityMetersPerSecond The max velocity.
+   */
+  public MaxVelocityConstraint(double maxVelocityMetersPerSecond) {
+    m_maxVelocity = maxVelocityMetersPerSecond;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    return m_maxVelocity;
+  }
+
+  @Override
+  public TrajectoryConstraint.MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
new file mode 100644
index 0000000..6758d3d
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+
+/**
+ * A class that enforces constraints on the mecanum drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for all 4 wheels of the drivetrain stay below a certain
+ * limit.
+ */
+public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final MecanumDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a mecanum drive dynamics constraint.
+   *
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics,
+                                               double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
+        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
new file mode 100644
index 0000000..c25c74c
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Enforces a particular constraint only within a rectangular region.
+ */
+public class RectangularRegionConstraint implements TrajectoryConstraint {
+  private final Translation2d m_bottomLeftPoint;
+  private final Translation2d m_topRightPoint;
+  private final TrajectoryConstraint m_constraint;
+
+  /**
+   * Constructs a new RectangularRegionConstraint.
+   *
+   * @param bottomLeftPoint The bottom left point of the rectangular region in which to
+   *                        enforce the constraint.
+   * @param topRightPoint   The top right point of the rectangular region in which to enforce
+   *                        the constraint.
+   * @param constraint      The constraint to enforce when the robot is within the region.
+   */
+  public RectangularRegionConstraint(Translation2d bottomLeftPoint, Translation2d topRightPoint,
+                                     TrajectoryConstraint constraint) {
+    m_bottomLeftPoint = bottomLeftPoint;
+    m_topRightPoint = topRightPoint;
+    m_constraint = constraint;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter,
+          velocityMetersPerSecond);
+    } else {
+      return Double.POSITIVE_INFINITY;
+    }
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters,
+          curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return new MinMax();
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the constraint
+   * is enforced in.
+   *
+   * @param robotPose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  public boolean isPoseInRegion(Pose2d robotPose) {
+    return robotPose.getX() >= m_bottomLeftPoint.getX()
+        && robotPose.getX() <= m_topRightPoint.getX()
+        && robotPose.getY() >= m_bottomLeftPoint.getY()
+        && robotPose.getY() <= m_topRightPoint.getY();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
new file mode 100644
index 0000000..693bfd5
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+
+/**
+ * A class that enforces constraints on the swerve drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for all 4 wheels of the drivetrain stay below a certain
+ * limit.
+ */
+public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final SwerveDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a swerve drive dynamics constraint.
+   *
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics,
+                                               double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                              double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
+        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
+    SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
+                                                       double curvatureRadPerMeter,
+                                                       double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
new file mode 100644
index 0000000..5962404
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory.constraint;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+
+/**
+ * An interface for defining user-defined velocity and acceleration constraints
+ * while generating trajectories.
+ */
+public interface TrajectoryConstraint {
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
+                                       double velocityMetersPerSecond);
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param poseMeters              The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter,
+                                                double velocityMetersPerSecond);
+
+  /**
+   * Represents a minimum and maximum acceleration.
+   */
+  @SuppressWarnings("MemberName")
+  class MinMax {
+    public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
+    public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
+
+    /**
+     * Constructs a MinMax.
+     *
+     * @param minAccelerationMetersPerSecondSq The minimum acceleration.
+     * @param maxAccelerationMetersPerSecondSq The maximum acceleration.
+     */
+    public MinMax(double minAccelerationMetersPerSecondSq,
+                  double maxAccelerationMetersPerSecondSq) {
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    /**
+     * Constructs a MinMax with default values.
+     */
+    public MinMax() {
+    }
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java
new file mode 100644
index 0000000..3f48306
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+/**
+ * Utility class that converts between commonly used units in FRC.
+ */
+public final class Units {
+  private static final double kInchesPerFoot = 12.0;
+  private static final double kMetersPerInch = 0.0254;
+  private static final double kSecondsPerMinute = 60;
+
+  /**
+   * Utility class, so constructor is private.
+   */
+  private Units() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Converts given meters to feet.
+   *
+   * @param meters The meters to convert to feet.
+   * @return Feet converted from meters.
+   */
+  public static double metersToFeet(double meters) {
+    return metersToInches(meters) / kInchesPerFoot;
+  }
+
+  /**
+   * Converts given feet to meters.
+   *
+   * @param feet The feet to convert to meters.
+   * @return Meters converted from feet.
+   */
+  public static double feetToMeters(double feet) {
+    return inchesToMeters(feet * kInchesPerFoot);
+  }
+
+  /**
+   * Converts given meters to inches.
+   *
+   * @param meters The meters to convert to inches.
+   * @return Inches converted from meters.
+   */
+  public static double metersToInches(double meters) {
+    return meters / kMetersPerInch;
+  }
+
+  /**
+   * Converts given inches to meters.
+   *
+   * @param inches The inches to convert to meters.
+   * @return Meters converted from inches.
+   */
+  public static double inchesToMeters(double inches) {
+    return inches * kMetersPerInch;
+  }
+
+  /**
+   * Converts given degrees to radians.
+   *
+   * @param degrees The degrees to convert to radians.
+   * @return Radians converted from degrees.
+   */
+  public static double degreesToRadians(double degrees) {
+    return Math.toRadians(degrees);
+  }
+
+  /**
+   * Converts given radians to degrees.
+   *
+   * @param radians The radians to convert to degrees.
+   * @return Degrees converted from radians.
+   */
+  public static double radiansToDegrees(double radians) {
+    return Math.toDegrees(radians);
+  }
+
+  /**
+   * Converts rotations per minute to radians per second.
+   *
+   * @param rpm The rotations per minute to convert to radians per second.
+   * @return Radians per second converted from rotations per minute.
+   */
+  public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
+    return rpm * Math.PI / (kSecondsPerMinute / 2);
+  }
+
+  /**
+   * Converts radians per second to rotations per minute.
+   *
+   * @param radiansPerSecond The radians per second to convert to from rotations per minute.
+   * @return Rotations per minute converted from radians per second.
+   */
+  public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
+    return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
new file mode 100644
index 0000000..d8b20a3
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import java.util.Objects;
+
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A class for constructing arbitrary RxC matrices.
+ *
+ * @param <R> The number of rows of the desired matrix.
+ * @param <C> The number of columns of the desired matrix.
+ */
+public class MatBuilder<R extends Num, C extends Num> {
+  final Nat<R> m_rows;
+  final Nat<C> m_cols;
+
+  /**
+   * Fills the matrix with the given data, encoded in row major form.
+   * (The matrix is filled row by row, left to right with the given data).
+   *
+   * @param data The data to fill the matrix with.
+   * @return The constructed matrix.
+   */
+  @SuppressWarnings("LineLength")
+  public final Matrix<R, C> fill(double... data) {
+    if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
+      throw new IllegalArgumentException("Invalid matrix data provided. Wanted " + this.m_rows.getNum()
+          + " x " + this.m_cols.getNum() + " matrix, but got " + data.length + " elements");
+    } else {
+      return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
+    }
+  }
+
+  /**
+   * Creates a new {@link MatBuilder} with the given dimensions.
+   * @param rows The number of rows of the matrix.
+   * @param cols The number of columns of the matrix.
+   */
+  public MatBuilder(Nat<R> rows, Nat<C> cols) {
+    this.m_rows = Objects.requireNonNull(rows);
+    this.m_cols = Objects.requireNonNull(cols);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
new file mode 100644
index 0000000..bd03f6b
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+public final class MathUtil {
+  private MathUtil() {
+    throw new AssertionError("utility class");
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low   The lower boundary to which to clamp value.
+   * @param high  The higher boundary to which to clamp value.
+   */
+  public static int clamp(int value, int low, int high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low   The lower boundary to which to clamp value.
+   * @param high  The higher boundary to which to clamp value.
+   */
+  public static double clamp(double value, double low, double high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
+  /**
+   * Constrains theta to within the range (-pi, pi].
+   *
+   * @param theta The angle to normalize.
+   * @return The normalized angle.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static double normalizeAngle(double theta) {
+    // Constraint theta to within (-3pi, pi)
+    int nPiPos = (int) ((theta + Math.PI) / 2.0 / Math.PI);
+    theta -= nPiPos * 2.0 * Math.PI;
+
+    // Cut off the bottom half of the above range to constrain within
+    // (-pi, pi]
+    int nPiNeg = (int) ((theta - Math.PI) / 2.0 / Math.PI);
+    theta -= nPiNeg * 2.0 * Math.PI;
+
+    return theta;
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
new file mode 100644
index 0000000..a87b98a
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
@@ -0,0 +1,678 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import java.util.Objects;
+
+import org.ejml.MatrixDimensionException;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import org.ejml.dense.row.MatrixFeatures_DDRM;
+import org.ejml.dense.row.NormOps_DDRM;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
+ *
+ * <p>This class is intended to be used alongside the state space library.
+ *
+ * @param <R> The number of rows in this matrix.
+ * @param <C> The number of columns in this matrix.
+ */
+@SuppressWarnings("PMD.ExcessivePublicCount")
+public class Matrix<R extends Num, C extends Num> {
+  protected final SimpleMatrix m_storage;
+
+  /**
+   * Constructs an empty zero matrix of the given dimensions.
+   *
+   * @param rows    The number of rows of the matrix.
+   * @param columns The number of columns of the matrix.
+   */
+  public Matrix(Nat<R> rows, Nat<C> columns) {
+    this.m_storage = new SimpleMatrix(
+      Objects.requireNonNull(rows).getNum(),
+      Objects.requireNonNull(columns).getNum()
+    );
+  }
+
+  /**
+   * Constructs a new {@link Matrix} with the given storage.
+   * Caller should make sure that the provided generic bounds match
+   * the shape of the provided {@link Matrix}.
+   *
+   * <p>NOTE:It is not recommend to use this constructor unless the
+   * {@link SimpleMatrix} API is absolutely necessary due to the desired
+   * function not being accessible through the {@link Matrix} wrapper.
+   *
+   * @param storage The {@link SimpleMatrix} to back this value.
+   */
+  public Matrix(SimpleMatrix storage) {
+    this.m_storage = Objects.requireNonNull(storage);
+  }
+
+  /**
+   * Constructs a new matrix with the storage of the supplied matrix.
+   *
+   * @param other The {@link Matrix} to copy the storage of.
+   */
+  public Matrix(Matrix<R, C> other) {
+    this.m_storage = Objects.requireNonNull(other).getStorage().copy();
+  }
+
+  /**
+   * Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps.
+   *
+   * <p>NOTE:The use of this method is heavily discouraged as this removes any
+   * guarantee of type safety. This should only be called if the {@link SimpleMatrix}
+   * API is absolutely necessary due to the desired function not being accessible through
+   * the {@link Matrix} wrapper.
+   *
+   * @return The underlying {@link SimpleMatrix} storage.
+   */
+  public SimpleMatrix getStorage() {
+    return m_storage;
+  }
+
+  /**
+   * Gets the number of columns in this matrix.
+   *
+   * @return The number of columns, according to the internal storage.
+   */
+  public final int getNumCols() {
+    return this.m_storage.numCols();
+  }
+
+  /**
+   * Gets the number of rows in this matrix.
+   *
+   * @return The number of rows, according to the internal storage.
+   */
+  public final int getNumRows() {
+    return this.m_storage.numRows();
+  }
+
+  /**
+   * Get an element of this matrix.
+   *
+   * @param row The row of the element.
+   * @param col The column of the element.
+   * @return The element in this matrix at row,col.
+   */
+  public final double get(int row, int col) {
+    return this.m_storage.get(row, col);
+  }
+
+  /**
+   * Sets the value at the given indices.
+   *
+   * @param row   The row of the element.
+   * @param col   The column of the element.
+   * @param value The value to insert at the given location.
+   */
+  public final void set(int row, int col, double value) {
+    this.m_storage.set(row, col, value);
+  }
+
+  /**
+   * Sets a row to a given row vector.
+   *
+   * @param row The row to set.
+   * @param val The row vector to set the given row to.
+   */
+  public final void setRow(int row, Matrix<N1, C> val) {
+    this.m_storage.setRow(row, 0,
+        Objects.requireNonNull(val).m_storage.getDDRM().getData());
+  }
+
+  /**
+   * Sets a column to a given column vector.
+   *
+   * @param column The column to set.
+   * @param val    The column vector to set the given row to.
+   */
+  public final void setColumn(int column, Matrix<R, N1> val) {
+    this.m_storage.setColumn(column, 0,
+        Objects.requireNonNull(val).m_storage.getDDRM().getData());
+  }
+
+
+  /**
+   * Sets all the elements in "this" matrix equal to the specified value.
+   *
+   * @param value The value each element is set to.
+   */
+  public void fill(double value) {
+    this.m_storage.fill(value);
+  }
+
+  /**
+   * Returns the diagonal elements inside a vector or square matrix.
+   *
+   * <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this"
+   * {@link Matrix} is a matrix then a vector of diagonal elements is returned.
+   *
+   * @return The diagonal elements inside a vector or a square matrix.
+   */
+  public final Matrix<R, C> diag() {
+    return new Matrix<>(this.m_storage.diag());
+  }
+
+  /**
+   * Returns the largest element of this matrix.
+   *
+   * @return The largest element of this matrix.
+   */
+  public final double max() {
+    return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Returns the absolute value of the element in this matrix with the largest absolute value.
+   *
+   * @return The absolute value of the element with the largest absolute value.
+   */
+  public final double maxAbs() {
+    return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM());
+  }
+
+
+  /**
+   * Returns the smallest element of this matrix.
+   *
+   * @return The smallest element of this matrix.
+   */
+  public final double minInternal() {
+    return CommonOps_DDRM.elementMin(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Calculates the mean of the elements in this matrix.
+   *
+   * @return The mean value of this matrix.
+   */
+  public final double mean() {
+    return this.elementSum() / (double) this.m_storage.getNumElements();
+  }
+
+  /**
+   * Multiplies this matrix with another that has C rows.
+   *
+   * <p>As matrix multiplication is only defined if the number of columns
+   * in the first matrix matches the number of rows in the second,
+   * this operation will fail to compile under any other circumstances.
+   *
+   * @param other The other matrix to multiply by.
+   * @param <C2>  The number of columns in the second matrix.
+   * @return The result of the matrix multiplication between "this" and the given matrix.
+   */
+  public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
+    return new Matrix<>(this.m_storage.mult(Objects.requireNonNull(other).m_storage));
+  }
+
+  /**
+   * Multiplies all the elements of this matrix by the given scalar.
+   *
+   * @param value The scalar value to multiply by.
+   * @return A new matrix with all the elements multiplied by the given value.
+   */
+  public Matrix<R, C> times(double value) {
+    return new Matrix<>(this.m_storage.scale(value));
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element multiplication of
+   * "this" and other.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
+   *
+   *
+   * @param other The other {@link Matrix} to preform element multiplication on.
+   * @return The element by element multiplication of "this" and other.
+   */
+  public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
+    return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
+  }
+
+  /**
+   * Subtracts the given value from all the elements of this matrix.
+   *
+   * @param value The value to subtract.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> minus(double value) {
+    return new Matrix<>(this.m_storage.minus(value));
+  }
+
+
+  /**
+   * Subtracts the given matrix from this matrix.
+   *
+   * @param value The matrix to subtract.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> minus(Matrix<R, C> value) {
+    return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
+  }
+
+
+  /**
+   * Adds the given value to all the elements of this matrix.
+   *
+   * @param value The value to add.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> plus(double value) {
+    return new Matrix<>(this.m_storage.plus(value));
+  }
+
+  /**
+   * Adds the given matrix to this matrix.
+   *
+   * @param value The matrix to add.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> plus(Matrix<R, C> value) {
+    return new Matrix<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Divides all elements of this matrix by the given value.
+   *
+   * @param value The value to divide by.
+   * @return The resultant matrix.
+   */
+  public Matrix<R, C> div(int value) {
+    return new Matrix<>(this.m_storage.divide((double) value));
+  }
+
+  /**
+   * Divides all elements of this matrix by the given value.
+   *
+   * @param value The value to divide by.
+   * @return The resultant matrix.
+   */
+  public Matrix<R, C> div(double value) {
+    return new Matrix<>(this.m_storage.divide(value));
+  }
+
+  /**
+   * Calculates the transpose, M^T of this matrix.
+   *
+   * @return The transpose matrix.
+   */
+  public final Matrix<C, R> transpose() {
+    return new Matrix<>(this.m_storage.transpose());
+  }
+
+
+  /**
+   * Returns a copy of this matrix.
+   *
+   * @return A copy of this matrix.
+   */
+  public final Matrix<R, C> copy() {
+    return new Matrix<>(this.m_storage.copy());
+  }
+
+
+  /**
+   * Returns the inverse matrix of "this" matrix.
+   *
+   * @return The inverse of "this" matrix.
+   * @throws org.ejml.data.SingularMatrixException If "this" matrix is non-invertable.
+   */
+  public final Matrix<R, C> inv() {
+    return new Matrix<>(this.m_storage.invert());
+  }
+
+  /**
+   * Returns the solution x to the equation Ax = b, where A is "this" matrix.
+   *
+   * <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the
+   * pseudo inverse is used if A is not square.
+   *
+   * @param b The right-hand side of the equation to solve.
+   * @return The solution to the linear system.
+   */
+  @SuppressWarnings("ParameterName")
+  public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
+    return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
+  }
+
+  /**
+   * Computes the matrix exponential using Eigen's solver.
+   * This method only works for square matrices, and will
+   * otherwise throw an {@link MatrixDimensionException}.
+   *
+   * @return The exponential of A.
+   */
+  public final Matrix<R, C> exp() {
+    if (this.getNumRows() != this.getNumCols()) {
+      throw new MatrixDimensionException("Non-square matrices cannot be exponentiated! "
+            + "This matrix is " + this.getNumRows() + " x " + this.getNumCols());
+    }
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.exp(this.m_storage.getDDRM().getData(), this.getNumRows(),
+          toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
+  /**
+   * Returns the determinant of this matrix.
+   *
+   * @return The determinant of this matrix.
+   */
+  public final double det() {
+    return this.m_storage.determinant();
+  }
+
+  /**
+   * Computes the Frobenius normal of the matrix.
+   *
+   * <p>normF = Sqrt{  &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>}   }
+   *
+   * @return The matrix's Frobenius normal.
+   */
+  public final double normF() {
+    return this.m_storage.normF();
+  }
+
+  /**
+   * Computes the induced p = 1 matrix norm.
+   *
+   * <p>||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
+   *
+   * @return The norm.
+   */
+  public final double normIndP1() {
+    return NormOps_DDRM.inducedP1(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Computes the sum of all the elements in the matrix.
+   *
+   * @return Sum of all the elements.
+   */
+  public final double elementSum() {
+    return this.m_storage.elementSum();
+  }
+
+  /**
+   * Computes the trace of the matrix.
+   *
+   * @return The trace of the matrix.
+   */
+  public final double trace() {
+    return this.m_storage.trace();
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element power of "this" and b.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
+   *
+   * @param b Scalar.
+   * @return The element by element power of "this" and b.
+   */
+  @SuppressWarnings("ParameterName")
+  public final Matrix<R, C> elementPower(double b) {
+    return new Matrix<>(this.m_storage.elementPower(b));
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element power of "this" and b.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
+   *
+   * @param b Scalar.
+   * @return The element by element power of "this" and b.
+   */
+  @SuppressWarnings("ParameterName")
+  public final Matrix<R, C> elementPower(int b) {
+    return new Matrix<>(this.m_storage.elementPower((double) b));
+  }
+
+  /**
+   * Extracts a given row into a row vector with new underlying storage.
+   *
+   * @param row The row to extract a vector from.
+   * @return A row vector from the given row.
+   */
+  public final Matrix<N1, C> extractRowVector(int row) {
+    return new Matrix<>(this.m_storage.extractVector(true, row));
+  }
+
+  /**
+   * Extracts a given column into a column vector with new underlying storage.
+   *
+   * @param column The column to extract a vector from.
+   * @return A column vector from the given column.
+   */
+  public final Matrix<R, N1> extractColumnVector(int column) {
+    return new Matrix<>(this.m_storage.extractVector(false, column));
+  }
+
+  /**
+   * Extracts a matrix of a given size and start position with new underlying
+   * storage.
+   *
+   * @param height The number of rows of the extracted matrix.
+   * @param width  The number of columns of the extracted matrix.
+   * @param startingRow The starting row of the extracted matrix.
+   * @param startingCol The starting column of the extracted matrix.
+   * @return The extracted matrix.
+   */
+  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
+      Nat<R2> height, Nat<C2> width, int startingRow, int startingCol) {
+    return new Matrix<>(this.m_storage.extractMatrix(
+      startingRow,
+      Objects.requireNonNull(height).getNum() + startingRow,
+      startingCol,
+      Objects.requireNonNull(width).getNum() + startingCol));
+  }
+
+  /**
+   * Assign a matrix of a given size and start position.
+   *
+   * @param startingRow The row to start at.
+   * @param startingCol  The column to start at.
+   * @param other  The matrix to assign the block to.
+   */
+  public <R2 extends Num, C2 extends Num> void assignBlock(int startingRow, int startingCol,
+                                                           Matrix<R2, C2> other) {
+    this.m_storage.insertIntoThis(
+        startingRow,
+        startingCol,
+        Objects.requireNonNull(other).m_storage);
+  }
+
+  /**
+   * Extracts a submatrix from the supplied matrix and inserts it in a submatrix in "this". The
+   * shape of "this" is used to determine the size of the matrix extracted.
+   *
+   * @param startingRow The starting row in the supplied matrix to extract the submatrix.
+   * @param startingCol The starting column in the supplied matrix to extract the submatrix.
+   * @param other       The matrix to extract the submatrix from.
+   */
+  public <R2 extends Num, C2 extends Num> void extractFrom(int startingRow, int startingCol,
+                                                           Matrix<R2, C2> other) {
+    CommonOps_DDRM.extract(other.m_storage.getDDRM(), startingRow, startingCol,
+        this.m_storage.getDDRM());
+  }
+
+  /**
+   * Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it
+   * will return the zero matrix.
+   *
+   * @param lowerTriangular Whether or not we want to decompose to the lower triangular
+   *                        Cholesky matrix.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
+   *                          semidefinite).
+   */
+  @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
+  public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
+    SimpleMatrix temp = m_storage.copy();
+
+    CholeskyDecomposition_F64<DMatrixRMaj> chol =
+            DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+    if (!chol.decompose(temp.getMatrix())) {
+      // check that the input is not all zeros -- if they are, we special case and return all
+      // zeros.
+      var matData = temp.getDDRM().data;
+      var isZeros = true;
+      for (double matDatum : matData) {
+        isZeros &= Math.abs(matDatum) < 1e-6;
+      }
+      if (isZeros) {
+        return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
+      }
+
+      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n"
+          + m_storage.toString());
+    }
+
+    return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
+  }
+
+  /**
+   * Returns the row major data of this matrix as a double array.
+   *
+   * @return The row major data of this matrix as a double array.
+   */
+  public double[] getData() {
+    return m_storage.getDDRM().getData();
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix as a {@link Nat}.
+   * @param <D> The dimension of the desired matrix as a generic.
+   * @return The DxD identity matrix.
+   */
+  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
+    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix as a {@link Num}.
+   * @param <D> The dimension of the desired matrix as a generic.
+   * @return The DxD identity matrix.
+   */
+  public static <D extends Num> Matrix<D, D> eye(D dim) {
+    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
+  }
+
+  /**
+   * Entrypoint to the {@link MatBuilder} class for creation
+   * of custom matrices with the given dimensions and contents.
+   *
+   * @param rows The number of rows of the desired matrix.
+   * @param cols The number of columns of the desired matrix.
+   * @param <R> The number of rows of the desired matrix as a generic.
+   * @param <C> The number of columns of the desired matrix as a generic.
+   * @return A builder to construct the matrix.
+   */
+  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
+    return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols));
+  }
+
+  /**
+   * Reassigns dimensions of a {@link Matrix} to allow for operations with
+   * other matrices that have wildcard dimensions.
+   *
+   * @param mat The {@link Matrix} to remove the dimensions from.
+   * @return The matrix with reassigned dimensions.
+   */
+  public static <R1 extends Num, C1 extends Num> Matrix<R1, C1> changeBoundsUnchecked(
+      Matrix<?, ?> mat) {
+    return new Matrix<>(mat.m_storage);
+  }
+
+  /**
+   * Checks if another {@link Matrix} is identical to "this" one within a specified tolerance.
+   *
+   * <p>This will check if each element is in tolerance of the corresponding element
+   * from the other {@link Matrix} or if the elements have the same symbolic meaning. For two
+   * elements to have the same symbolic meaning they both must be either Double.NaN,
+   * Double.POSITIVE_INFINITY, or Double.NEGATIVE_INFINITY.
+   *
+   * <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this
+   * method when checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)}
+   * will return false if an element is uncountable. This method should only be used when
+   * uncountable elements need to compared.
+   *
+   * @param other     The {@link Matrix} to check against this one.
+   * @param tolerance The tolerance to check equality with.
+   * @return true if this matrix is identical to the one supplied.
+   */
+  public boolean isIdentical(Matrix<?, ?> other, double tolerance) {
+    return MatrixFeatures_DDRM.isIdentical(this.m_storage.getDDRM(),
+        other.m_storage.getDDRM(), tolerance);
+  }
+
+  /**
+   * Checks if another {@link Matrix} is equal to "this" within a specified tolerance.
+   *
+   * <p>This will check if each element is in tolerance of the corresponding element
+   * from the other {@link Matrix}.
+   *
+   * <p>tol &ge; |a<sub>ij</sub> - b<sub>ij</sub>|
+   *
+   * @param other     The {@link Matrix} to check against this one.
+   * @param tolerance The tolerance to check equality with.
+   * @return true if this matrix is equal to the one supplied.
+   */
+  public boolean isEqual(Matrix<?, ?> other, double tolerance) {
+    return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(),
+        other.m_storage.getDDRM(), tolerance);
+  }
+
+  @Override
+  public String toString() {
+    return m_storage.toString();
+  }
+
+  /**
+   * Checks if an object is equal to this {@link Matrix}.
+   *
+   * <p>a<sub>ij</sub> == b<sub>ij</sub>
+   *
+   * @param other The Object to check against this {@link Matrix}.
+   * @return true if the object supplied is a {@link Matrix} and is equal to this matrix.
+   */
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (!(other instanceof Matrix)) {
+      return false;
+    }
+
+    Matrix<?, ?> matrix = (Matrix<?, ?>) other;
+    if (MatrixFeatures_DDRM.hasUncountable(matrix.m_storage.getDDRM())) {
+      return false;
+    }
+    return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), matrix.m_storage.getDDRM());
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_storage);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
new file mode 100644
index 0000000..b3e4724
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import java.util.Objects;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+@Deprecated
+public final class MatrixUtils {
+  private MatrixUtils() {
+    throw new AssertionError("utility class");
+  }
+
+  /**
+   * Creates a new matrix of zeros.
+   *
+   * @param rows The number of rows in the matrix.
+   * @param cols The number of columns in the matrix.
+   * @param <R> The number of rows in the matrix as a generic.
+   * @param <C> The number of columns in the matrix as a generic.
+   * @return An RxC matrix filled with zeros.
+   */
+  @SuppressWarnings("LineLength")
+  public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
+    return new Matrix<>(
+        new SimpleMatrix(Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
+  }
+
+  /**
+   * Creates a new vector of zeros.
+   *
+   * @param nums The size of the desired vector.
+   * @param <N> The size of the desired vector as a generic.
+   * @return A vector of size N filled with zeros.
+   */
+  public static <N extends Num> Matrix<N, N1> zeros(Nat<N> nums) {
+    return new Matrix<>(new SimpleMatrix(Objects.requireNonNull(nums).getNum(), 1));
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix.
+   * @param <D> The dimension of the desired matrix as a generic.
+   * @return The DxD identity matrix.
+   */
+  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
+    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
+  }
+
+  /**
+   * Entrypoint to the MatBuilder class for creation
+   * of custom matrices with the given dimensions and contents.
+   *
+   * @param rows The number of rows of the desired matrix.
+   * @param cols The number of columns of the desired matrix.
+   * @param <R> The number of rows of the desired matrix as a generic.
+   * @param <C> The number of columns of the desired matrix as a generic.
+   * @return A builder to construct the matrix.
+   */
+  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
+    return new MatBuilder<>(rows, cols);
+  }
+
+  /**
+   * Entrypoint to the VecBuilder class for creation
+   * of custom vectors with the given size and contents.
+   *
+   * @param dim The dimension of the vector.
+   * @param <D> The dimension of the vector as a generic.
+   * @return A builder to construct the vector.
+   */
+  public static <D extends Num> VecBuilder<D> vec(Nat<D> dim) {
+    return new VecBuilder<>(dim);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java
new file mode 100644
index 0000000..0b8a81f
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+/**
+ * A number expressed as a java class.
+ */
+public abstract class Num {
+  /**
+   * The number this is backing.
+   *
+   * @return The number represented by this class.
+   */
+  public abstract int getNum();
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java
new file mode 100644
index 0000000..eafbcba
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+public class Pair<A, B> {
+  private final A m_first;
+  private final B m_second;
+
+  public Pair(A first, B second) {
+    m_first = first;
+    m_second = second;
+  }
+
+  public A getFirst() {
+    return m_first;
+  }
+
+  public B getSecond() {
+    return m_second;
+  }
+
+  @SuppressWarnings("ParameterName")
+  public static <A, B> Pair<A, B> of(A a, B b) {
+    return new Pair<>(a, b);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
new file mode 100644
index 0000000..3f281d2
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import java.util.function.BiFunction;
+
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.NormOps_DDRM;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
+import org.ejml.simple.SimpleBase;
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.math.WPIMathJNI;
+
+public final class SimpleMatrixUtils {
+  private SimpleMatrixUtils() {
+  }
+
+  /**
+   * Compute the matrix exponential, e^M of the given matrix.
+   *
+   * @param matrix The matrix to compute the exponential of.
+   * @return The resultant matrix.
+   */
+  @SuppressWarnings({"LocalVariableName", "LineLength"})
+  public static SimpleMatrix expm(SimpleMatrix matrix) {
+    BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
+    SimpleMatrix A = matrix;
+    double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
+    int n_squarings = 0;
+
+    if (A_L1 < 1.495585217958292e-002) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 2.539398330063230e-001) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 9.504178996162932e-001) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 2.097847961257068e+000) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else {
+      double maxNorm = 5.371920351148152;
+      double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
+      n_squarings = (int) Math.max(0, Math.ceil(log));
+      A = A.divide(Math.pow(2.0, n_squarings));
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade13(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    }
+  }
+
+  @SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
+  private static SimpleMatrix dispatchPade(SimpleMatrix U, SimpleMatrix V,
+                                           int nSquarings, BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
+    SimpleMatrix P = U.plus(V);
+    SimpleMatrix Q = U.negative().plus(V);
+
+    SimpleMatrix R = solveProvider.apply(Q, P);
+
+    for (int i = 0; i < nSquarings; i++) {
+      R = R.mult(R);
+    }
+
+    return R;
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
+    double[] b = new double[]{120, 60, 12, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
+    SimpleMatrix V = A2.scale(b[2]).plus(ident.scale(b[0]));
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
+    double[] b = new double[]{30240, 15120, 3360, 420, 30, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+
+    SimpleMatrix U = A.mult(A4.scale(b[5]).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V = A4.scale(b[4]).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
+    double[] b = new double[]{17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+
+    SimpleMatrix U =
+            A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V =
+            A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
+    double[] b = new double[]{17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240,
+        2162160, 110880, 3960, 90, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+    SimpleMatrix A8 = A6.mult(A2);
+
+    SimpleMatrix U =
+            A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V =
+            A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
+    double[] b = new double[]{64764752532480000.0, 32382376266240000.0, 7771770303897600.0,
+        1187353796428800.0, 129060195264000.0, 10559470521600.0, 670442572800.0,
+        33522128640.0, 1323241920, 40840800, 960960, 16380, 182, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+
+    SimpleMatrix U =
+            A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V =
+            A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
+
+    return new Pair<>(U, V);
+  }
+
+  private static SimpleMatrix eye(int rows, int cols) {
+    return SimpleMatrix.identity(Math.min(rows, cols));
+  }
+
+  /**
+   * The identy of a square matrix.
+   *
+   * @param rows the number of rows (and columns)
+   * @return the identiy matrix, rows x rows.
+   */
+  public static SimpleMatrix eye(int rows) {
+    return SimpleMatrix.identity(rows);
+  }
+
+  /**
+   * Decompose the given matrix using Cholesky Decomposition and return a view of the upper
+   * triangular matrix (if you want lower triangular see the other overload of this method.) If the
+   * input matrix is zeros, this will return the zero matrix.
+   *
+   * @param src The matrix to decompose.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+   *                          semidefinite).
+   */
+  public static SimpleMatrix lltDecompose(SimpleMatrix src) {
+    return lltDecompose(src, false);
+  }
+
+  /**
+   * Decompose the given matrix using Cholesky Decomposition. If the input matrix is zeros, this
+   * will return the zero matrix.
+   *
+   * @param src The matrix to decompose.
+   * @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+   *                          semidefinite).
+   */
+  @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
+  public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
+    SimpleMatrix temp = src.copy();
+
+    CholeskyDecomposition_F64<DMatrixRMaj> chol =
+            DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+    if (!chol.decompose(temp.getMatrix())) {
+      // check that the input is not all zeros -- if they are, we special case and return all
+      // zeros.
+      var matData = temp.getDDRM().data;
+      var isZeros = true;
+      for (double matDatum : matData) {
+        isZeros &= Math.abs(matDatum) < 1e-6;
+      }
+      if (isZeros) {
+        return new SimpleMatrix(temp.numRows(), temp.numCols());
+      }
+
+      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
+    }
+
+    return SimpleMatrix.wrap(chol.getT(null));
+  }
+
+  /**
+   * Computes the matrix exponential using Eigen's solver.
+   *
+   * @param A the matrix to exponentiate.
+   * @return the exponential of A.
+   */
+  @SuppressWarnings("ParameterName")
+  public static SimpleMatrix exp(
+          SimpleMatrix A) {
+    SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
+    WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
+    return toReturn;
+  }
+
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
new file mode 100644
index 0000000..98200b7
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
@@ -0,0 +1,177 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N10;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N4;
+import edu.wpi.first.wpiutil.math.numbers.N5;
+import edu.wpi.first.wpiutil.math.numbers.N6;
+import edu.wpi.first.wpiutil.math.numbers.N7;
+import edu.wpi.first.wpiutil.math.numbers.N8;
+import edu.wpi.first.wpiutil.math.numbers.N9;
+
+
+
+/**
+ * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
+ *
+ * @param <N> The dimension of the vector to be constructed.
+ */
+public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
+  public VecBuilder(Nat<N> rows) {
+    super(rows, Nat.N1());
+  }
+
+  private Vector<N> fillVec(double... data) {
+    return new Vector<>(fill(data));
+  }
+
+  /**
+   * Returns a 1x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   */
+  public static Vector<N1> fill(double n1) {
+    return new VecBuilder<>(Nat.N1()).fillVec(n1);
+  }
+
+  /**
+   * Returns a 2x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   */
+  public static Vector<N2> fill(double n1, double n2) {
+    return new VecBuilder<>(Nat.N2()).fillVec(n1, n2);
+  }
+
+  /**
+   * Returns a 3x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   */
+  public static Vector<N3> fill(double n1, double n2, double n3) {
+    return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3);
+  }
+
+  /**
+   * Returns a 4x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   */
+  public static Vector<N4> fill(double n1, double n2, double n3, double n4) {
+    return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4);
+  }
+
+  /**
+   * Returns a 5x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   */
+  public static Vector<N5> fill(double n1, double n2, double n3, double n4, double n5) {
+    return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5);
+  }
+
+  /**
+   * Returns a 6x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   */
+  public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5,
+                                    double n6) {
+    return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6);
+  }
+
+  /**
+   * Returns a 7x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   */
+  public static Vector<N7> fill(double n1, double n2, double n3, double n4, double n5,
+                                    double n6, double n7) {
+    return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7);
+  }
+
+  /**
+   * Returns a 8x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   */
+  public static Vector<N8> fill(double n1, double n2, double n3, double n4, double n5,
+                                    double n6, double n7, double n8) {
+    return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8);
+  }
+
+  /**
+   * Returns a 9x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   * @param n9 the ninth element.
+   */
+  public static Vector<N9> fill(double n1, double n2, double n3, double n4, double n5,
+                                    double n6, double n7, double n8, double n9) {
+    return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
+  }
+
+  /**
+   * Returns a 10x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   * @param n9 the ninth element.
+   * @param n10 the tenth element.
+   */
+  @SuppressWarnings("PMD.ExcessiveParameterList")
+  public static Vector<N10> fill(double n1, double n2, double n3, double n4, double n5,
+                                    double n6, double n7, double n8, double n9, double n10) {
+    return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
+  }
+}

wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java

diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java
new file mode 100644
index 0000000..04f46ba
--- /dev/null
+++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+/**
+ * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
+ *
+ * <p>This class is intended to be used alongside the state space library.
+ *
+ * @param <R> The number of rows in this matrix.
+ */
+public class Vector<R extends Num> extends Matrix<R, N1> {
+
+  /**
+   * Constructs an empty zero vector of the given dimensions.
+   *
+   * @param rows    The number of rows of the vector.
+   */
+  public Vector(Nat<R> rows) {
+    super(rows, Nat.N1());
+  }
+
+  /**
+   * Constructs a new {@link Vector} with the given storage.
+   * Caller should make sure that the provided generic bounds match
+   * the shape of the provided {@link Vector}.
+   *
+   * <p>NOTE:It is not recommended to use this constructor unless the
+   * {@link SimpleMatrix} API is absolutely necessary due to the desired
+   * function not being accessible through the {@link Vector} wrapper.
+   *
+   * @param storage The {@link SimpleMatrix} to back this vector.
+   */
+  public Vector(SimpleMatrix storage) {
+    super(storage);
+  }
+
+  /**
+   * Constructs a new vector with the storage of the supplied matrix.
+   *
+   * @param other The {@link Vector} to copy the storage of.
+   */
+  public Vector(Matrix<R, N1> other) {
+    super(other);
+  }
+
+  @Override
+  public Vector<R> times(double value) {
+    return new Vector<>(this.m_storage.scale(value));
+  }
+
+  @Override
+  public Vector<R> div(int value) {
+    return new Vector<>(this.m_storage.divide(value));
+  }
+
+  @Override
+  public Vector<R> div(double value) {
+    return new Vector<>(this.m_storage.divide(value));
+  }
+}

wpimath/src/main/native/cpp/MathShared.cpp

diff --git a/wpimath/src/main/native/cpp/MathShared.cpp b/wpimath/src/main/native/cpp/MathShared.cpp
new file mode 100644
index 0000000..8a64f2e
--- /dev/null
+++ b/wpimath/src/main/native/cpp/MathShared.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "wpimath/MathShared.h"
+
+#include <wpi/mutex.h>
+
+using namespace wpi::math;
+
+namespace {
+class DefaultMathShared : public MathShared {
+ public:
+  void ReportError(const wpi::Twine& error) override {}
+  void ReportUsage(MathUsageId id, int count) override {}
+};
+}  // namespace
+
+static std::unique_ptr<MathShared> mathShared;
+static wpi::mutex setLock;
+
+MathShared& MathSharedStore::GetMathShared() {
+  std::scoped_lock lock(setLock);
+  if (!mathShared) mathShared = std::make_unique<DefaultMathShared>();
+  return *mathShared;
+}
+
+void MathSharedStore::SetMathShared(std::unique_ptr<MathShared> shared) {
+  std::scoped_lock lock(setLock);
+  mathShared = std::move(shared);
+}

wpimath/src/main/native/cpp/StateSpaceUtil.cpp

diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
new file mode 100644
index 0000000..d828f30
--- /dev/null
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/StateSpaceUtil.h"
+
+namespace frc {
+
+template <>
+bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
+                          const Eigen::Matrix<double, 1, 1>& B) {
+  return detail::IsStabilizableImpl<1, 1>(A, B);
+}
+
+template <>
+bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
+                          const Eigen::Matrix<double, 2, 1>& B) {
+  return detail::IsStabilizableImpl<2, 1>(A, B);
+}
+
+Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose) {
+  return frc::MakeMatrix<3, 1>(pose.X().to<double>(), pose.Y().to<double>(),
+                               pose.Rotation().Radians().to<double>());
+}
+
+}  // namespace frc

wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp

diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
new file mode 100644
index 0000000..ae58440
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/LinearQuadraticRegulator.h"
+
+namespace frc {
+
+LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+    const std::array<double, 1>& Qelems, const std::array<double, 1>& Relems,
+    units::second_t dt)
+    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                               MakeCostMatrix(Relems), dt) {}
+
+LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+    const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
+
+LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const std::array<double, 2>& Qelems, const std::array<double, 1>& Relems,
+    units::second_t dt)
+    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                               MakeCostMatrix(Relems), dt) {}
+
+LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
+
+}  // namespace frc

wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp

diff --git a/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
new file mode 100644
index 0000000..88e7e66
--- /dev/null
+++ b/wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
@@ -0,0 +1,87 @@
+// This file contains the implementation of both drake_assert and drake_throw.
+/* clang-format off to disable clang-format-includes */
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_throw.h"
+/* clang-format on */
+
+#include <atomic>
+#include <cstdlib>
+#include <iostream>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+
+#include "drake/common/drake_assertion_error.h"
+#include "drake/common/never_destroyed.h"
+
+namespace drake {
+namespace internal {
+namespace {
+
+// Singleton to manage assertion configuration.
+struct AssertionConfig {
+  static AssertionConfig& singleton() {
+    static never_destroyed<AssertionConfig> global;
+    return global.access();
+  }
+
+  std::atomic<bool> assertion_failures_are_exceptions;
+};
+
+// Stream into @p out the given failure details; only @p condition may be null.
+void PrintFailureDetailTo(std::ostream& out, const char* condition,
+                          const char* func, const char* file, int line) {
+  out << "Failure at " << file << ":" << line << " in " << func << "()";
+  if (condition) {
+    out << ": condition '" << condition << "' failed.";
+  } else {
+    out << ".";
+  }
+}
+}  // namespace
+
+// Declared in drake_assert.h.
+void Abort(const char* condition, const char* func, const char* file,
+           int line) {
+  std::cerr << "abort: ";
+  PrintFailureDetailTo(std::cerr, condition, func, file, line);
+  std::cerr << std::endl;
+  std::abort();
+}
+
+// Declared in drake_throw.h.
+void Throw(const char* condition, const char* func, const char* file,
+           int line) {
+  std::ostringstream what;
+  PrintFailureDetailTo(what, condition, func, file, line);
+  throw assertion_error(what.str().c_str());
+}
+
+// Declared in drake_assert.h.
+void AssertionFailed(const char* condition, const char* func, const char* file,
+                     int line) {
+  if (AssertionConfig::singleton().assertion_failures_are_exceptions) {
+    Throw(condition, func, file, line);
+  } else {
+    Abort(condition, func, file, line);
+  }
+}
+
+}  // namespace internal
+}  // namespace drake
+
+// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling
+// behavior.
+//
+// By default, assertion failures will result in an ::abort().  If this method
+// has ever been called, failures will result in a thrown exception instead.
+//
+// Assertion configuration has process-wide scope.  Changes here will affect
+// all assertions within the current process.
+//
+// This method is intended ONLY for use by pydrake bindings, and thus is not
+// declared in any header file, to discourage anyone from using it.
+extern "C" void drake_set_assertion_failure_to_throw_exception() {
+  drake::internal::AssertionConfig::singleton().
+      assertion_failures_are_exceptions = true;
+}

wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp

diff --git a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
new file mode 100644
index 0000000..e80cadc
--- /dev/null
+++ b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
@@ -0,0 +1,463 @@
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_throw.h"
+#include "drake/common/is_approx_equal_abstol.h"
+
+#include <Eigen/Eigenvalues>
+#include <Eigen/QR>
+
+// This code has https://github.com/RobotLocomotion/drake/pull/11118 applied to
+// fix an infinite loop in reorder_eigen().
+
+namespace drake {
+namespace math {
+namespace {
+/* helper functions */
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B) {
+  // This function checks if (A,B) is a stabilizable pair.
+  // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
+  // A, if any, have absolute values less than one, where an eigenvalue is
+  // uncontrollable if Rank[lambda * I - A, B] < n.
+  int n = B.rows(), m = B.cols();
+  Eigen::EigenSolver<Eigen::MatrixXd> es(A);
+  for (int i = 0; i < n; i++) {
+    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
+            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
+        1)
+      continue;
+    Eigen::MatrixXcd E(n, n + m);
+    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
+    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
+    DRAKE_THROW_UNLESS(qr.rank() == n);
+  }
+}
+void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                      const Eigen::Ref<const Eigen::MatrixXd>& Q) {
+  // This function check if (A,C) is a detectable pair, where Q = C' * C.
+  // (A,C) is detectable if and only if the unobservable eigenvalues of A,
+  // if any, have absolute values less than one, where an eigenvalue is
+  // unobservable if Rank[lambda * I - A; C] < n.
+  // Also, (A,C) is detectable if and only if (A',C') is stabilizable.
+  int n = A.rows();
+  Eigen::LDLT<Eigen::MatrixXd> ldlt(Q);
+  Eigen::MatrixXd L = ldlt.matrixL();
+  Eigen::MatrixXd D = ldlt.vectorD();
+  Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n);
+  for (int i = 0; i < n; i++) {
+    D_sqrt(i, i) = sqrt(D(i));
+  }
+  Eigen::MatrixXd C = L * D_sqrt;
+  check_stabilizable(A.transpose(), C.transpose());
+}
+
+// "Givens rotation" computes an orthogonal 2x2 matrix R such that
+// it eliminates the 2nd coordinate of the vector [a,b]', i.e.,
+// R * [ a ] = [ a_hat ]
+//     [ b ]   [   0   ]
+// The implementation is based on
+// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation
+void Givens_rotation(double a, double b, Eigen::Ref<Eigen::Matrix2d> R,
+                     double eps = 1e-10) {
+  double c, s;
+  if (fabs(b) < eps) {
+    c = (a < -eps ? -1 : 1);
+    s = 0;
+  } else if (fabs(a) < eps) {
+    c = 0;
+    s = -sgn(b);
+  } else if (fabs(a) > fabs(b)) {
+    double t = b / a;
+    double u = sgn(a) * fabs(sqrt(1 + t * t));
+    c = 1 / u;
+    s = -c * t;
+  } else {
+    double t = a / b;
+    double u = sgn(b) * fabs(sqrt(1 + t * t));
+    s = -1 / u;
+    c = -s * t;
+  }
+  R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c;
+}
+
+// The arguments S, T, and Z will be changed.
+void swap_block_11(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
+                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
+  // Dooren, Case I, p124-125.
+  int n2 = S.rows();
+  Eigen::Matrix2d A = S.block<2, 2>(p, p);
+  Eigen::Matrix2d B = T.block<2, 2>(p, p);
+  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A;
+  Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p));
+  S = (S * Z1).eval();
+  T = (T * Z1).eval();
+  Z = (Z * Z1).eval();
+  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2);
+  Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p));
+  S = (Q * S).eval();
+  T = (Q * T).eval();
+  S(p + 1, p) = 0;
+  T(p + 1, p) = 0;
+}
+
+// The arguments S, T, and Z will be changed.
+void swap_block_21(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
+                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
+  // Dooren, Case II, p126-127.
+  int n2 = S.rows();
+  Eigen::Matrix3d A = S.block<3, 3>(p, p);
+  Eigen::Matrix3d B = T.block<3, 3>(p, p);
+  // Compute H and eliminate H(1,0) by row operation.
+  Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A;
+  Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
+  Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0));
+  H = (R * H).eval();
+  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
+  // Compute Z1, Z2, Q1, Q2.
+  Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1));
+  H = (H * Z1.block<3, 3>(p, p)).eval();
+  Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p));
+  S = (S * Z1).eval();
+  T = (T * Z1).eval();
+  Z = (Z * Z1 * Z2).eval();
+  Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1),
+                  Q1.block<2, 2>(p + 1, p + 1));
+  S = (Q1 * S * Z2).eval();
+  T = (Q1 * T * Z2).eval();
+  Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p));
+  S = (Q2 * S).eval();
+  T = (Q2 * T).eval();
+  S(p + 1, p) = 0;
+  S(p + 2, p) = 0;
+  T(p + 1, p) = 0;
+  T(p + 2, p) = 0;
+  T(p + 2, p + 1) = 0;
+}
+
+// The arguments S, T, and Z will be changed.
+void swap_block_12(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
+                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
+  int n2 = S.rows();
+  // Swap the role of S and T.
+  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
+  Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1),
+                  Q0.block<2, 2>(p + 1, p + 1));
+  S = (Q0 * S).eval();
+  T = (Q0 * T).eval();
+  Eigen::Matrix3d A = S.block<3, 3>(p, p);
+  Eigen::Matrix3d B = T.block<3, 3>(p, p);
+  // Compute H and eliminate H(2,1) by column operation.
+  Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B;
+  Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3);
+  Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1));
+  H = (H * R).eval();
+  // Compute Q1, Q2, Z1, Z2.
+  Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p));
+  H = (Q1.block<3, 3>(p, p) * H).eval();
+  Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1));
+  S = (Q1 * S).eval();
+  T = (Q1 * T).eval();
+  Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p));
+  S = (Q2 * S * Z1).eval();
+  T = (Q2 * T * Z1).eval();
+  Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1),
+                  Z2.block<2, 2>(p + 1, p + 1));
+  S = (S * Z2).eval();
+  T = (T * Z2).eval();
+  Z = (Z * Z1 * Z2).eval();
+  // Swap back the role of S and T.
+  Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
+  S = (Q3 * S).eval();
+  T = (Q3 * T).eval();
+  S(p + 2, p) = 0;
+  S(p + 2, p + 1) = 0;
+  T(p + 1, p) = 0;
+  T(p + 2, p) = 0;
+  T(p + 2, p + 1) = 0;
+}
+
+// The arguments S, T, and Z will be changed.
+void swap_block_22(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
+                   Eigen::Ref<Eigen::MatrixXd> Z, int p) {
+  // Direct Swapping Algorithm based on
+  // "Numerical Methods for General and Structured Eigenvalue Problems" by
+  // Daniel Kressner, p108-111.
+  // ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ).
+  // Also relevant but not applicable here:
+  // "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James
+  // W. Demmelt;
+  int n2 = S.rows();
+  Eigen::MatrixXd A = S.block<4, 4>(p, p);
+  Eigen::MatrixXd B = T.block<4, 4>(p, p);
+  // Solve
+  // A11 * X - Y A22 = A12
+  // B11 * X - Y B22 = B12
+  // Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4].
+  Eigen::Matrix<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
+  Eigen::Matrix<double, 8, 1> D;
+  // clang-format off
+  C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0,
+       0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0,
+       A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2),
+       0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3),
+       B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0,
+       0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0,
+       B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2),
+       0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3);
+  // clang-format on
+  D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3);
+  Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D);
+  // Q * [ -Y ] = [ R_Y ] ,  Z' * [ -X ] = [ R_X ] .
+  //     [ I  ]   [  0  ]         [ I  ] = [  0  ]
+  Eigen::Matrix<double, 4, 2> X, Y;
+  X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity();
+  Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity();
+  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2);
+  Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr1(X);
+  Z1.block<4, 4>(p, p) = qr1.householderQ();
+  Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > qr2(Y);
+  Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint();
+  // Apply transform Q1 * (S,T) * Z1.
+  S = (Q1 * S * Z1).eval();
+  T = (Q1 * T * Z1).eval();
+  Z = (Z * Z1).eval();
+  // Eliminate the T(p+3,p+2) entry.
+  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2);
+  Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2),
+                  Q2.block<2, 2>(p + 2, p + 2));
+  S = (Q2 * S).eval();
+  T = (Q2 * T).eval();
+  // Eliminate the T(p+1,p) entry.
+  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2);
+  Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p));
+  S = (Q3 * S).eval();
+  T = (Q3 * T).eval();
+  S(p + 2, p) = 0;
+  S(p + 2, p + 1) = 0;
+  S(p + 3, p) = 0;
+  S(p + 3, p + 1) = 0;
+  T(p + 1, p) = 0;
+  T(p + 2, p) = 0;
+  T(p + 2, p + 1) = 0;
+  T(p + 3, p) = 0;
+  T(p + 3, p + 1) = 0;
+  T(p + 3, p + 2) = 0;
+}
+
+// Functionality of "swap_block" function:
+// swap the 1x1 or 2x2 blocks pointed by p and q.
+// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1
+// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices.
+// Algorithms are described in the papers
+// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
+// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and
+// "Numerical Methods for General and Structured Eigenvalue Problems" by
+// Daniel Kressner, 2005.
+void swap_block(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
+                Eigen::Ref<Eigen::MatrixXd> Z, int p, int q, int q_block_size,
+                double eps = 1e-10) {
+  int p_tmp = q, p_block_size;
+  while (p_tmp-- > p) {
+    p_block_size = 1;
+    if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) {
+      p_block_size = 2;
+      p_tmp--;
+    }
+    switch (p_block_size * 10 + q_block_size) {
+      case 11:
+        swap_block_11(S, T, Z, p_tmp);
+        break;
+      case 21:
+        swap_block_21(S, T, Z, p_tmp);
+        break;
+      case 12:
+        swap_block_12(S, T, Z, p_tmp);
+        break;
+      case 22:
+        swap_block_22(S, T, Z, p_tmp);
+        break;
+    }
+  }
+}
+
+// Functionality of "reorder_eigen" function:
+// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has
+// stable eigenvalues by multiplying Q's and Z's on the left and the right,
+// respectively.
+// Stable eigenvalues are inside the unit disk.
+//
+// Algorithm:
+// Go along the diagonals of (S,T) from the top left to the bottom right.
+// Once find a stable eigenvalue, push it to top left.
+// In implementation, use two pointers, p and q.
+// p points to the current block (1x1 or 2x2) and q points to the block with the
+// stable eigenvalue(s).
+// Push the block pointed by q to the position pointed by p.
+// Finish when n stable eigenvalues are placed at the top-left n by n matrix.
+// The algorithm for swapping blocks is described in the papers
+// "A generalized eigenvalue approach for solving Riccati equations" by P. Van
+// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue
+// Problems" by Daniel Kressner, 2005.
+void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
+                   Eigen::Ref<Eigen::MatrixXd> Z, double eps = 1e-10) {
+  // abs(a) < eps => a = 0
+  int n2 = S.rows();
+  int n = n2 / 2, p = 0, q = 0;
+
+  // Find the first unstable p block.
+  while (p < n) {
+    if (fabs(S(p + 1, p)) < eps) {  // p block size = 1
+      if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) {  // stable
+        p++;
+        continue;
+      }
+    } else {  // p block size = 2
+      const double det_T =
+          T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1);
+      if (fabs(det_T) > eps) {
+        const double det_S =
+            S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1);
+        if (fabs(det_S) <= fabs(det_T)) {  // stable
+          p += 2;
+          continue;
+        }
+      }
+    }
+    break;
+  }
+  q = p;
+
+  // Make the first n generalized eigenvalues stable.
+  while (p < n && q < n2) {
+    // Update q.
+    int q_block_size = 0;
+    while (q < n2) {
+      if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) {  // block size = 1
+        if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) {
+          q_block_size = 1;
+          break;
+        }
+        q++;
+      } else {  // block size = 2
+        const double det_T =
+            T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1);
+        if (fabs(det_T) > eps) {
+          const double det_S =
+              S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1);
+          if (fabs(det_S) <= fabs(det_T)) {
+            q_block_size = 2;
+            break;
+          }
+        }
+        q += 2;
+      }
+    }
+    if (q >= n2)
+      throw std::runtime_error("fail to find enough stable eigenvalues");
+    // Swap blocks pointed by p and q.
+    if (p != q) {
+      swap_block(S, T, Z, p, q, q_block_size);
+      p += q_block_size;
+      q += q_block_size;
+    }
+  }
+  if (p < n && q >= n2)
+    throw std::runtime_error("fail to find enough stable eigenvalues");
+}
+}  // namespace
+
+/**
+ * DiscreteAlgebraicRiccatiEquation function
+ * computes the unique stabilizing solution X to the discrete-time algebraic
+ * Riccati equation:
+ * \f[
+ * A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
+ * \f]
+ *
+ * @throws std::runtime_error if Q is not positive semi-definite.
+ * @throws std::runtime_error if R is not positive definite.
+ *
+ * Based on the Schur Vector approach outlined in this paper:
+ * "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
+ * by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell, in TAC, 1980,
+ * http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434
+ *
+ * Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
+ * R_half are sampled from standard normal distributions, where
+ * Q = Q_half'*Q_half and similar for R, the absolute error of the solution
+ * is 10^{-6}, while the absolute error of the solution computed by Matlab is
+ * 10^{-8}.
+ *
+ * TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
+ * accuracy, together with more thorough tests.
+ */
+
+Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+    const Eigen::Ref<const Eigen::MatrixXd>& A,
+    const Eigen::Ref<const Eigen::MatrixXd>& B,
+    const Eigen::Ref<const Eigen::MatrixXd>& Q,
+    const Eigen::Ref<const Eigen::MatrixXd>& R) {
+  int n = B.rows(), m = B.cols();
+
+  DRAKE_DEMAND(m <= n);
+  DRAKE_DEMAND(A.rows() == n && A.cols() == n);
+  DRAKE_DEMAND(Q.rows() == n && Q.cols() == n);
+  DRAKE_DEMAND(R.rows() == m && R.cols() == m);
+  DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10));
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(Q);
+  for (int i = 0; i < n; i++) {
+    DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0);
+  }
+  DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10));
+  Eigen::LLT<Eigen::MatrixXd> R_cholesky(R);
+  DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success);
+  check_stabilizable(A, B);
+  check_detectable(A, Q);
+
+  Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n);
+  M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n);
+  L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(),
+      Eigen::MatrixXd::Zero(n, n), A.transpose();
+
+  // QZ decomposition of M and L
+  // QMZ = S, QLZ = T
+  // where Q and Z are real orthogonal matrixes
+  // T is upper-triangular matrix, and S is upper quasi-triangular matrix
+  Eigen::RealQZ<Eigen::MatrixXd> qz(2 * n);
+  qz.compute(M, L);  // M = Q S Z,  L = Q T Z (Q and Z computed by Eigen package
+                     // are adjoints of Q and Z above)
+  Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(),
+                  Z = qz.matrixZ().adjoint();
+
+  // Reorder the generalized eigenvalues of (S,T).
+  Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n);
+  reorder_eigen(S, T, Z2);
+  Z = (Z * Z2).eval();
+
+  // The first n columns of Z is ( U1 ) .
+  //                             ( U2 )
+  //            -1
+  // X = U2 * U1   is a solution of the discrete time Riccati equation.
+  Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n);
+  Eigen::MatrixXd X = U2 * U1.inverse();
+  X = (X + X.adjoint().eval()) / 2.0;
+  return X;
+}
+
+}  // namespace math
+}  // namespace drake

wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp

diff --git a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
new file mode 100644
index 0000000..a1747ab
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/estimator/KalmanFilter.h"
+
+namespace frc {
+
+KalmanFilter<1, 1, 1>::KalmanFilter(
+    LinearSystem<1, 1, 1>& plant, const std::array<double, 1>& stateStdDevs,
+    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
+                                        dt} {}
+
+KalmanFilter<2, 1, 1>::KalmanFilter(
+    LinearSystem<2, 1, 1>& plant, const std::array<double, 2>& stateStdDevs,
+    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
+                                        dt} {}
+
+}  // namespace frc

wpimath/src/main/native/cpp/geometry/Pose2d.cpp

diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
new file mode 100644
index 0000000..57dad44
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Pose2d.h"
+
+#include <cmath>
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
+    : m_translation(translation), m_rotation(rotation) {}
+
+Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
+    : m_translation(x, y), m_rotation(rotation) {}
+
+Pose2d Pose2d::operator+(const Transform2d& other) const {
+  return TransformBy(other);
+}
+
+Pose2d& Pose2d::operator+=(const Transform2d& other) {
+  m_translation += other.Translation().RotateBy(m_rotation);
+  m_rotation += other.Rotation();
+  return *this;
+}
+
+Transform2d Pose2d::operator-(const Pose2d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform2d(pose.Translation(), pose.Rotation());
+}
+
+bool Pose2d::operator==(const Pose2d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Pose2d::operator!=(const Pose2d& other) const {
+  return !operator==(other);
+}
+
+Pose2d Pose2d::TransformBy(const Transform2d& other) const {
+  return {m_translation + (other.Translation().RotateBy(m_rotation)),
+          m_rotation + other.Rotation()};
+}
+
+Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
+  const Transform2d transform{other, *this};
+  return {transform.Translation(), transform.Rotation()};
+}
+
+Pose2d Pose2d::Exp(const Twist2d& twist) const {
+  const auto dx = twist.dx;
+  const auto dy = twist.dy;
+  const auto dtheta = twist.dtheta.to<double>();
+
+  const auto sinTheta = std::sin(dtheta);
+  const auto cosTheta = std::cos(dtheta);
+
+  double s, c;
+  if (std::abs(dtheta) < 1E-9) {
+    s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+    c = 0.5 * dtheta;
+  } else {
+    s = sinTheta / dtheta;
+    c = (1 - cosTheta) / dtheta;
+  }
+
+  const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
+                              Rotation2d{cosTheta, sinTheta}};
+
+  return *this + transform;
+}
+
+Twist2d Pose2d::Log(const Pose2d& end) const {
+  const auto transform = end.RelativeTo(*this);
+  const auto dtheta = transform.Rotation().Radians().to<double>();
+  const auto halfDtheta = dtheta / 2.0;
+
+  const auto cosMinusOne = transform.Rotation().Cos() - 1;
+
+  double halfThetaByTanOfHalfDtheta;
+
+  if (std::abs(cosMinusOne) < 1E-9) {
+    halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+  } else {
+    halfThetaByTanOfHalfDtheta =
+        -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
+  }
+
+  const Translation2d translationPart =
+      transform.Translation().RotateBy(
+          {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
+      std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
+
+  return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
+}
+
+void frc::to_json(wpi::json& json, const Pose2d& pose) {
+  json = wpi::json{{"translation", pose.Translation()},
+                   {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose2d& pose) {
+  pose = Pose2d{json.at("translation").get<Translation2d>(),
+                json.at("rotation").get<Rotation2d>()};
+}

wpimath/src/main/native/cpp/geometry/Rotation2d.cpp

diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
new file mode 100644
index 0000000..32a9b40
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Rotation2d.h"
+
+#include <cmath>
+
+#include <wpi/json.h>
+
+#include "units/math.h"
+
+using namespace frc;
+
+Rotation2d::Rotation2d(units::radian_t value)
+    : m_value(value),
+      m_cos(units::math::cos(value)),
+      m_sin(units::math::sin(value)) {}
+
+Rotation2d::Rotation2d(units::degree_t value)
+    : m_value(value),
+      m_cos(units::math::cos(value)),
+      m_sin(units::math::sin(value)) {}
+
+Rotation2d::Rotation2d(double x, double y) {
+  const auto magnitude = std::hypot(x, y);
+  if (magnitude > 1e-6) {
+    m_sin = y / magnitude;
+    m_cos = x / magnitude;
+  } else {
+    m_sin = 0.0;
+    m_cos = 1.0;
+  }
+  m_value = units::radian_t(std::atan2(m_sin, m_cos));
+}
+
+Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+  return RotateBy(other);
+}
+
+Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
+  double cos = Cos() * other.Cos() - Sin() * other.Sin();
+  double sin = Cos() * other.Sin() + Sin() * other.Cos();
+  m_cos = cos;
+  m_sin = sin;
+  m_value = units::radian_t(std::atan2(m_sin, m_cos));
+  return *this;
+}
+
+Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
+  return *this + -other;
+}
+
+Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
+  *this += -other;
+  return *this;
+}
+
+Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
+
+Rotation2d Rotation2d::operator*(double scalar) const {
+  return Rotation2d(m_value * scalar);
+}
+
+bool Rotation2d::operator==(const Rotation2d& other) const {
+  return units::math::abs(m_value - other.m_value) < 1E-9_rad;
+}
+
+bool Rotation2d::operator!=(const Rotation2d& other) const {
+  return !operator==(other);
+}
+
+Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
+  return {Cos() * other.Cos() - Sin() * other.Sin(),
+          Cos() * other.Sin() + Sin() * other.Cos()};
+}
+
+void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
+  json = wpi::json{{"radians", rotation.Radians().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
+  rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
+}

wpimath/src/main/native/cpp/geometry/Transform2d.cpp

diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
new file mode 100644
index 0000000..eb5e2ec
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Transform2d.h"
+
+#include "frc/geometry/Pose2d.h"
+
+using namespace frc;
+
+Transform2d::Transform2d(Pose2d initial, Pose2d final) {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  m_translation = (final.Translation() - initial.Translation())
+                      .RotateBy(-initial.Rotation());
+
+  m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
+    : m_translation(translation), m_rotation(rotation) {}
+
+Transform2d Transform2d::Inverse() const {
+  // We are rotating the difference between the translations
+  // using a clockwise rotation matrix. This transforms the global
+  // delta into a local delta (relative to the initial pose).
+  return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
+bool Transform2d::operator==(const Transform2d& other) const {
+  return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Transform2d::operator!=(const Transform2d& other) const {
+  return !operator==(other);
+}

wpimath/src/main/native/cpp/geometry/Translation2d.cpp

diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
new file mode 100644
index 0000000..6f4551c
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Translation2d.h"
+
+#include <wpi/json.h>
+
+#include "units/math.h"
+
+using namespace frc;
+
+Translation2d::Translation2d(units::meter_t x, units::meter_t y)
+    : m_x(x), m_y(y) {}
+
+Translation2d::Translation2d(units::meter_t distance, const Rotation2d& angle)
+    : m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
+
+units::meter_t Translation2d::Distance(const Translation2d& other) const {
+  return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
+}
+
+units::meter_t Translation2d::Norm() const {
+  return units::math::hypot(m_x, m_y);
+}
+
+Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
+  return {m_x * other.Cos() - m_y * other.Sin(),
+          m_x * other.Sin() + m_y * other.Cos()};
+}
+
+Translation2d Translation2d::operator+(const Translation2d& other) const {
+  return {X() + other.X(), Y() + other.Y()};
+}
+
+Translation2d& Translation2d::operator+=(const Translation2d& other) {
+  m_x += other.m_x;
+  m_y += other.m_y;
+  return *this;
+}
+
+Translation2d Translation2d::operator-(const Translation2d& other) const {
+  return *this + -other;
+}
+
+Translation2d& Translation2d::operator-=(const Translation2d& other) {
+  *this += -other;
+  return *this;
+}
+
+Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
+
+Translation2d Translation2d::operator*(double scalar) const {
+  return {scalar * m_x, scalar * m_y};
+}
+
+Translation2d& Translation2d::operator*=(double scalar) {
+  m_x *= scalar;
+  m_y *= scalar;
+  return *this;
+}
+
+Translation2d Translation2d::operator/(double scalar) const {
+  return *this * (1.0 / scalar);
+}
+
+bool Translation2d::operator==(const Translation2d& other) const {
+  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+         units::math::abs(m_y - other.m_y) < 1E-9_m;
+}
+
+bool Translation2d::operator!=(const Translation2d& other) const {
+  return !operator==(other);
+}
+
+Translation2d& Translation2d::operator/=(double scalar) {
+  *this *= (1.0 / scalar);
+  return *this;
+}
+
+void frc::to_json(wpi::json& json, const Translation2d& translation) {
+  json = wpi::json{{"x", translation.X().to<double>()},
+                   {"y", translation.Y().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation2d& translation) {
+  translation = Translation2d{units::meter_t{json.at("x").get<double>()},
+                              units::meter_t{json.at("y").get<double>()}};
+}

wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp

diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
new file mode 100644
index 0000000..26d57e2
--- /dev/null
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "Eigen/Core"
+#include "Eigen/Eigenvalues"
+#include "Eigen/QR"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "edu_wpi_first_math_WPIMathJNI.h"
+#include "unsupported/Eigen/MatrixFunctions"
+
+using namespace wpi::java;
+
+bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
+                        const Eigen::Ref<const Eigen::MatrixXd>& B) {
+  // This function checks if (A,B) is a stabilizable pair.
+  // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
+  // A, if any, have absolute values less than one, where an eigenvalue is
+  // uncontrollable if Rank[lambda * I - A, B] < n.
+  int n = B.rows(), m = B.cols();
+  Eigen::EigenSolver<Eigen::MatrixXd> es(A);
+  for (int i = 0; i < n; i++) {
+    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
+            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
+        1)
+      continue;
+
+    Eigen::MatrixXcd E(n, n + m);
+    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
+    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
+    if (qr.rank() != n) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    discreteAlgebraicRiccatiEquation
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
+  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
+  jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
+  jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Amat{nativeA, states, states};
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Bmat{nativeB, states, inputs};
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Qmat{nativeQ, states, states};
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Rmat{nativeR, inputs, inputs};
+
+  Eigen::MatrixXd result =
+      drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
+
+  env->ReleaseDoubleArrayElements(A, nativeA, 0);
+  env->ReleaseDoubleArrayElements(B, nativeB, 0);
+  env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
+  env->ReleaseDoubleArrayElements(R, nativeR, 0);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    exp
+ * Signature: ([DI[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_exp
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
+{
+  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Amat{arrayBody, rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Aexp =
+      Amat.exp();
+
+  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    isStabilizable
+ * Signature: (II[D[D)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
+  (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
+   jdoubleArray bSrc)
+{
+  jdouble* nativeA = env->GetDoubleArrayElements(aSrc, nullptr);
+  jdouble* nativeB = env->GetDoubleArrayElements(bSrc, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      A{nativeA, states, states};
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      B{nativeB, states, inputs};
+
+  bool isStabilizable = check_stabilizable(A, B);
+
+  env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
+  env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);
+
+  return isStabilizable;
+}
+
+}  // extern "C"

wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp

diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
new file mode 100644
index 0000000..25b6c51
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+DifferentialDriveOdometry::DifferentialDriveOdometry(
+    const Rotation2d& gyroAngle, const Pose2d& initialPose)
+    : m_pose(initialPose) {
+  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
+}
+
+const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
+                                                units::meter_t leftDistance,
+                                                units::meter_t rightDistance) {
+  auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
+  auto deltaRightDistance = rightDistance - m_prevRightDistance;
+
+  m_prevLeftDistance = leftDistance;
+  m_prevRightDistance = rightDistance;
+
+  auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+  auto angle = gyroAngle + m_gyroOffset;
+
+  auto newPose = m_pose.Exp(
+      {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
+
+  m_previousAngle = angle;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}

wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp

diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
new file mode 100644
index 0000000..36a4952
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+void DifferentialDriveWheelSpeeds::Normalize(
+    units::meters_per_second_t attainableMaxSpeed) {
+  auto realMaxSpeed =
+      units::math::max(units::math::abs(left), units::math::abs(right));
+
+  if (realMaxSpeed > attainableMaxSpeed) {
+    left = left / realMaxSpeed * attainableMaxSpeed;
+    right = right / realMaxSpeed * attainableMaxSpeed;
+  }
+}

wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp

diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
new file mode 100644
index 0000000..de1b2d0
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+
+using namespace frc;
+
+MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
+    const ChassisSpeeds& chassisSpeeds,
+    const Translation2d& centerOfRotation) const {
+  // We have a new center of rotation. We need to compute the matrix again.
+  if (centerOfRotation != m_previousCoR) {
+    auto fl = m_frontLeftWheel - centerOfRotation;
+    auto fr = m_frontRightWheel - centerOfRotation;
+    auto rl = m_rearLeftWheel - centerOfRotation;
+    auto rr = m_rearRightWheel - centerOfRotation;
+
+    SetInverseKinematics(fl, fr, rl, rr);
+
+    m_previousCoR = centerOfRotation;
+  }
+
+  Eigen::Vector3d chassisSpeedsVector;
+  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
+      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+
+  Eigen::Matrix<double, 4, 1> wheelsMatrix =
+      m_inverseKinematics * chassisSpeedsVector;
+
+  MecanumDriveWheelSpeeds wheelSpeeds;
+  wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
+  wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
+  wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
+  wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
+  return wheelSpeeds;
+}
+
+ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
+    const MecanumDriveWheelSpeeds& wheelSpeeds) const {
+  Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
+  // clang-format off
+  wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
+                       wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
+  // clang-format on
+
+  Eigen::Vector3d chassisSpeedsVector =
+      m_forwardKinematics.solve(wheelSpeedsMatrix);
+
+  return {units::meters_per_second_t{chassisSpeedsVector(0)},
+          units::meters_per_second_t{chassisSpeedsVector(1)},
+          units::radians_per_second_t{chassisSpeedsVector(2)}};
+}
+
+void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
+                                                  Translation2d fr,
+                                                  Translation2d rl,
+                                                  Translation2d rr) const {
+  // clang-format off
+  m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
+                         1,  1, (fr.X() - fr.Y()).template to<double>(),
+                         1,  1, (rl.X() - rl.Y()).template to<double>(),
+                         1, -1, (-(rr.X() + rr.Y())).template to<double>();
+  // clang-format on
+  m_inverseKinematics /= std::sqrt(2);
+}

wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp

diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
new file mode 100644
index 0000000..7534fc1
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveOdometry.h"
+
+#include "wpimath/MathShared.h"
+
+using namespace frc;
+
+MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+                                           const Rotation2d& gyroAngle,
+                                           const Pose2d& initialPose)
+    : m_kinematics(kinematics), m_pose(initialPose) {
+  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
+}
+
+const Pose2d& MecanumDriveOdometry::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    MecanumDriveWheelSpeeds wheelSpeeds) {
+  units::second_t deltaTime =
+      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
+  m_previousTime = currentTime;
+
+  auto angle = gyroAngle + m_gyroOffset;
+
+  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+  static_cast<void>(dtheta);
+
+  auto newPose = m_pose.Exp(
+      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+  m_previousAngle = angle;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}

wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp

diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
new file mode 100644
index 0000000..b20dddf
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+
+#include "units/math.h"
+
+using namespace frc;
+
+void MecanumDriveWheelSpeeds::Normalize(
+    units::meters_per_second_t attainableMaxSpeed) {
+  std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
+                                                        rearLeft, rearRight};
+  units::meters_per_second_t realMaxSpeed = *std::max_element(
+      wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
+        return units::math::abs(a) < units::math::abs(b);
+      });
+
+  if (realMaxSpeed > attainableMaxSpeed) {
+    for (int i = 0; i < 4; ++i) {
+      wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
+    }
+    frontLeft = wheelSpeeds[0];
+    frontRight = wheelSpeeds[1];
+    rearLeft = wheelSpeeds[2];
+    rearRight = wheelSpeeds[3];
+  }
+}

wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp

diff --git a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
new file mode 100644
index 0000000..c00a362
--- /dev/null
+++ b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/CubicHermiteSpline.h"
+
+using namespace frc;
+
+CubicHermiteSpline::CubicHermiteSpline(
+    std::array<double, 2> xInitialControlVector,
+    std::array<double, 2> xFinalControlVector,
+    std::array<double, 2> yInitialControlVector,
+    std::array<double, 2> yFinalControlVector) {
+  const auto hermite = MakeHermiteBasis();
+  const auto x =
+      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+  const auto y =
+      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+  // Populate first two rows with coefficients.
+  m_coefficients.template block<1, 4>(0, 0) = hermite * x;
+  m_coefficients.template block<1, 4>(1, 0) = hermite * y;
+
+  // Populate Row 2 and Row 3 with the derivatives of the equations above.
+  // Then populate row 4 and 5 with the second derivatives.
+  for (int i = 0; i < 4; i++) {
+    // Here, we are multiplying by (3 - i) to manually take the derivative. The
+    // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
+    m_coefficients.template block<2, 1>(2, i) =
+        m_coefficients.template block<2, 1>(0, i) * (3 - i);
+  }
+
+  for (int i = 0; i < 3; i++) {
+    // Here, we are multiplying by (2 - i) to manually take the derivative. The
+    // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
+    m_coefficients.template block<2, 1>(4, i) =
+        m_coefficients.template block<2, 1>(2, i) * (2 - i);
+  }
+}

wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp

diff --git a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
new file mode 100644
index 0000000..5b34cdb
--- /dev/null
+++ b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/QuinticHermiteSpline.h"
+
+using namespace frc;
+
+QuinticHermiteSpline::QuinticHermiteSpline(
+    std::array<double, 3> xInitialControlVector,
+    std::array<double, 3> xFinalControlVector,
+    std::array<double, 3> yInitialControlVector,
+    std::array<double, 3> yFinalControlVector) {
+  const auto hermite = MakeHermiteBasis();
+  const auto x =
+      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+  const auto y =
+      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+  // Populate first two rows with coefficients.
+  m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
+  m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
+
+  // Populate Row 2 and Row 3 with the derivatives of the equations above.
+  // Then populate row 4 and 5 with the second derivatives.
+  for (int i = 0; i < 6; i++) {
+    // Here, we are multiplying by (5 - i) to manually take the derivative. The
+    // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
+    m_coefficients.template block<2, 1>(2, i) =
+        m_coefficients.template block<2, 1>(0, i) * (5 - i);
+  }
+  for (int i = 0; i < 5; i++) {
+    // Here, we are multiplying by (4 - i) to manually take the derivative. The
+    // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
+    m_coefficients.template block<2, 1>(4, i) =
+        m_coefficients.template block<2, 1>(2, i) * (4 - i);
+  }
+}

wpimath/src/main/native/cpp/spline/SplineHelper.cpp

diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
new file mode 100644
index 0000000..58f7537
--- /dev/null
+++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineHelper.h"
+
+#include <cstddef>
+
+using namespace frc;
+
+std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
+    const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
+    const Spline<3>::ControlVector& end) {
+  std::vector<CubicHermiteSpline> splines;
+
+  std::array<double, 2> xInitial = start.x;
+  std::array<double, 2> yInitial = start.y;
+  std::array<double, 2> xFinal = end.x;
+  std::array<double, 2> yFinal = end.y;
+
+  if (waypoints.size() > 1) {
+    waypoints.emplace(waypoints.begin(),
+                      Translation2d{units::meter_t(xInitial[0]),
+                                    units::meter_t(yInitial[0])});
+    waypoints.emplace_back(
+        Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
+
+    // Populate tridiagonal system for clamped cubic
+    /* See:
+    https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
+    /undervisningsmateriale/chap7alecture.pdf
+    */
+
+    // Above-diagonal of tridiagonal matrix, zero-padded
+    std::vector<double> a;
+    // Diagonal of tridiagonal matrix
+    std::vector<double> b(waypoints.size() - 2, 4.0);
+    // Below-diagonal of tridiagonal matrix, zero-padded
+    std::vector<double> c;
+    // rhs vectors
+    std::vector<double> dx, dy;
+    // solution vectors
+    std::vector<double> fx(waypoints.size() - 2, 0.0),
+        fy(waypoints.size() - 2, 0.0);
+
+    // populate above-diagonal and below-diagonal vectors
+    a.emplace_back(0);
+    for (size_t i = 0; i < waypoints.size() - 3; ++i) {
+      a.emplace_back(1);
+      c.emplace_back(1);
+    }
+    c.emplace_back(0);
+
+    // populate rhs vectors
+    dx.emplace_back(
+        3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
+        xInitial[1]);
+    dy.emplace_back(
+        3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
+        yInitial[1]);
+    if (waypoints.size() > 4) {
+      for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
+        // dx and dy represent the derivatives of the internal waypoints. The
+        // derivative of the second internal waypoint should involve the third
+        // and first internal waypoint, which have indices of 1 and 3 in the
+        // waypoints list (which contains ALL waypoints).
+        dx.emplace_back(3 * (waypoints[i + 2].X().to<double>() -
+                             waypoints[i].X().to<double>()));
+        dy.emplace_back(3 * (waypoints[i + 2].Y().to<double>() -
+                             waypoints[i].Y().to<double>()));
+      }
+    }
+    dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
+                         waypoints[waypoints.size() - 3].X().to<double>()) -
+                    xFinal[1]);
+    dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
+                         waypoints[waypoints.size() - 3].Y().to<double>()) -
+                    yFinal[1]);
+
+    // Compute solution to tridiagonal system
+    ThomasAlgorithm(a, b, c, dx, &fx);
+    ThomasAlgorithm(a, b, c, dy, &fy);
+
+    fx.emplace(fx.begin(), xInitial[1]);
+    fx.emplace_back(xFinal[1]);
+    fy.emplace(fy.begin(), yInitial[1]);
+    fy.emplace_back(yFinal[1]);
+
+    for (size_t i = 0; i < fx.size() - 1; ++i) {
+      // Create the spline.
+      const CubicHermiteSpline spline{
+          {waypoints[i].X().to<double>(), fx[i]},
+          {waypoints[i + 1].X().to<double>(), fx[i + 1]},
+          {waypoints[i].Y().to<double>(), fy[i]},
+          {waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
+
+      splines.push_back(spline);
+    }
+  } else if (waypoints.size() == 1) {
+    const double xDeriv =
+        (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
+    const double yDeriv =
+        (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
+
+    std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
+                                            xDeriv};
+    std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
+                                            yDeriv};
+
+    splines.emplace_back(xInitial, midXControlVector, yInitial,
+                         midYControlVector);
+    splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
+
+  } else {
+    // Create the spline.
+    const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
+    splines.push_back(spline);
+  }
+
+  return splines;
+}
+
+std::vector<QuinticHermiteSpline>
+SplineHelper::QuinticSplinesFromControlVectors(
+    const std::vector<Spline<5>::ControlVector>& controlVectors) {
+  std::vector<QuinticHermiteSpline> splines;
+  for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
+    auto& xInitial = controlVectors[i].x;
+    auto& yInitial = controlVectors[i].y;
+    auto& xFinal = controlVectors[i + 1].x;
+    auto& yFinal = controlVectors[i + 1].y;
+    splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
+  }
+  return splines;
+}
+
+std::array<Spline<3>::ControlVector, 2>
+SplineHelper::CubicControlVectorsFromWaypoints(
+    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+    const Pose2d& end) {
+  double scalar;
+  if (interiorWaypoints.empty()) {
+    scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
+  } else {
+    scalar =
+        1.2 *
+        start.Translation().Distance(interiorWaypoints.front()).to<double>();
+  }
+  const auto initialCV = CubicControlVector(scalar, start);
+  if (!interiorWaypoints.empty()) {
+    scalar =
+        1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
+  }
+  const auto finalCV = CubicControlVector(scalar, end);
+  return {initialCV, finalCV};
+}
+
+std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
+    const std::vector<Pose2d>& waypoints) {
+  std::vector<QuinticHermiteSpline> splines;
+  splines.reserve(waypoints.size() - 1);
+  for (size_t i = 0; i < waypoints.size() - 1; ++i) {
+    auto& p0 = waypoints[i];
+    auto& p1 = waypoints[i + 1];
+
+    // This just makes the splines look better.
+    const auto scalar =
+        1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
+
+    auto controlVectorA = QuinticControlVector(scalar, p0);
+    auto controlVectorB = QuinticControlVector(scalar, p1);
+    splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y,
+                         controlVectorB.y);
+  }
+  return splines;
+}
+
+void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
+                                   const std::vector<double>& b,
+                                   const std::vector<double>& c,
+                                   const std::vector<double>& d,
+                                   std::vector<double>* solutionVector) {
+  auto& f = *solutionVector;
+  size_t N = d.size();
+
+  // Create the temporary vectors
+  // Note that this is inefficient as it is possible to call
+  // this function many times. A better implementation would
+  // pass these temporary matrices by non-const reference to
+  // save excess allocation and deallocation
+  std::vector<double> c_star(N, 0.0);
+  std::vector<double> d_star(N, 0.0);
+
+  // This updates the coefficients in the first row
+  // Note that we should be checking for division by zero here
+  c_star[0] = c[0] / b[0];
+  d_star[0] = d[0] / b[0];
+
+  // Create the c_star and d_star coefficients in the forward sweep
+  for (size_t i = 1; i < N; ++i) {
+    double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
+    c_star[i] = c[i] * m;
+    d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
+  }
+
+  f[N - 1] = d_star[N - 1];
+  // This is the reverse sweep, used to update the solution vector f
+  for (int i = N - 2; i >= 0; i--) {
+    f[i] = d_star[i] - c_star[i] * f[i + 1];
+  }
+}

wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp

diff --git a/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
new file mode 100644
index 0000000..b7e7f9e
--- /dev/null
+++ b/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineParameterizer.h"
+
+constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
+constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
+constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;

wpimath/src/main/native/cpp/trajectory/Trajectory.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
new file mode 100644
index 0000000..067b8de
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/Trajectory.h"
+
+#include <algorithm>
+
+#include <wpi/json.h>
+
+#include "units/math.h"
+
+using namespace frc;
+
+bool Trajectory::State::operator==(const Trajectory::State& other) const {
+  return t == other.t && velocity == other.velocity &&
+         acceleration == other.acceleration && pose == other.pose &&
+         curvature == other.curvature;
+}
+
+bool Trajectory::State::operator!=(const Trajectory::State& other) const {
+  return !operator==(other);
+}
+
+Trajectory::State Trajectory::State::Interpolate(State endValue,
+                                                 double i) const {
+  // Find the new [t] value.
+  const auto newT = Lerp(t, endValue.t, i);
+
+  // Find the delta time between the current state and the interpolated state.
+  const auto deltaT = newT - t;
+
+  // If delta time is negative, flip the order of interpolation.
+  if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
+
+  // Check whether the robot is reversing at this stage.
+  const auto reversing =
+      velocity < 0_mps ||
+      (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
+
+  // Calculate the new velocity.
+  // v = v_0 + at
+  const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
+
+  // Calculate the change in position.
+  // delta_s = v_0 t + 0.5 at^2
+  const units::meter_t newS =
+      (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
+      (reversing ? -1.0 : 1.0);
+
+  // Return the new state. To find the new position for the new state, we need
+  // to interpolate between the two endpoint poses. The fraction for
+  // interpolation is the change in position (delta s) divided by the total
+  // distance between the two endpoints.
+  const double interpolationFrac =
+      newS / endValue.pose.Translation().Distance(pose.Translation());
+
+  return {newT, newV, acceleration,
+          Lerp(pose, endValue.pose, interpolationFrac),
+          Lerp(curvature, endValue.curvature, interpolationFrac)};
+}
+
+Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
+  m_totalTime = states.back().t;
+}
+
+Trajectory::State Trajectory::Sample(units::second_t t) const {
+  if (t <= m_states.front().t) return m_states.front();
+  if (t >= m_totalTime) return m_states.back();
+
+  // Use binary search to get the element with a timestamp no less than the
+  // requested timestamp. This starts at 1 because we use the previous state
+  // later on for interpolation.
+  auto sample =
+      std::lower_bound(m_states.cbegin() + 1, m_states.cend(), t,
+                       [](const auto& a, const auto& b) { return a.t < b; });
+
+  auto prevSample = sample - 1;
+
+  // The sample's timestamp is now greater than or equal to the requested
+  // timestamp. If it is greater, we need to interpolate between the
+  // previous state and the current state to get the exact state that we
+  // want.
+
+  // If the difference in states is negligible, then we are spot on!
+  if (units::math::abs(sample->t - prevSample->t) < 1E-9_s) {
+    return *sample;
+  }
+  // Interpolate between the two states for the state that we want.
+  return prevSample->Interpolate(
+      *sample, (t - prevSample->t) / (sample->t - prevSample->t));
+}
+
+Trajectory Trajectory::TransformBy(const Transform2d& transform) {
+  auto& firstState = m_states[0];
+  auto& firstPose = firstState.pose;
+
+  // Calculate the transformed first pose.
+  auto newFirstPose = firstPose + transform;
+  auto newStates = m_states;
+  newStates[0].pose = newFirstPose;
+
+  for (unsigned int i = 1; i < newStates.size(); i++) {
+    auto& state = newStates[i];
+    // We are transforming relative to the coordinate frame of the new initial
+    // pose.
+    state.pose = newFirstPose + (state.pose - firstPose);
+  }
+
+  return Trajectory(newStates);
+}
+
+Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
+  auto newStates = m_states;
+  for (auto& state : newStates) {
+    state.pose = state.pose.RelativeTo(pose);
+  }
+  return Trajectory(newStates);
+}
+
+void frc::to_json(wpi::json& json, const Trajectory::State& state) {
+  json = wpi::json{{"time", state.t.to<double>()},
+                   {"velocity", state.velocity.to<double>()},
+                   {"acceleration", state.acceleration.to<double>()},
+                   {"pose", state.pose},
+                   {"curvature", state.curvature.to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Trajectory::State& state) {
+  state.pose = json.at("pose").get<Pose2d>();
+  state.t = units::second_t{json.at("time").get<double>()};
+  state.velocity =
+      units::meters_per_second_t{json.at("velocity").get<double>()};
+  state.acceleration =
+      units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
+  state.curvature = units::curvature_t{json.at("curvature").get<double>()};
+}
+
+bool Trajectory::operator==(const Trajectory& other) const {
+  return m_states == other.States();
+}
+
+bool Trajectory::operator!=(const Trajectory& other) const {
+  return !operator==(other);
+}

wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
new file mode 100644
index 0000000..6cb3f7a
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -0,0 +1,142 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryGenerator.h"
+
+#include <utility>
+
+#include <wpi/raw_ostream.h>
+
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
+    std::vector<Trajectory::State>{Trajectory::State()});
+std::function<void(const char*)> TrajectoryGenerator::s_errorFunc;
+
+void TrajectoryGenerator::ReportError(const char* error) {
+  if (s_errorFunc)
+    s_errorFunc(error);
+  else
+    wpi::errs() << "TrajectoryGenerator error: " << error << "\n";
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    Spline<3>::ControlVector initial,
+    const std::vector<Translation2d>& interiorWaypoints,
+    Spline<3>::ControlVector end, const TrajectoryConfig& config) {
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+
+  // Make theta normal for trajectory generation if path is reversed.
+  // Flip the headings.
+  if (config.IsReversed()) {
+    initial.x[1] *= -1;
+    initial.y[1] *= -1;
+    end.x[1] *= -1;
+    end.y[1] *= -1;
+  }
+
+  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points =
+        SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+            initial, interiorWaypoints, end));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (config.IsReversed()) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+    const Pose2d& end, const TrajectoryConfig& config) {
+  auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
+      start, interiorWaypoints, end);
+  return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    std::vector<Spline<5>::ControlVector> controlVectors,
+    const TrajectoryConfig& config) {
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  // Make theta normal for trajectory generation if path is reversed.
+  if (config.IsReversed()) {
+    for (auto& vector : controlVectors) {
+      // Flip the headings.
+      vector.x[1] *= -1;
+      vector.y[1] *= -1;
+    }
+  }
+
+  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points = SplinePointsFromSplines(
+        SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (config.IsReversed()) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
+  auto newWaypoints = waypoints;
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  if (config.IsReversed())
+    for (auto& waypoint : newWaypoints) waypoint += flip;
+
+  std::vector<SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points = SplinePointsFromSplines(
+        SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (config.IsReversed()) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
+}

wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
new file mode 100644
index 0000000..0e78a15
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -0,0 +1,237 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+#include <string>
+
+#include "units/math.h"
+
+using namespace frc;
+
+Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
+    const std::vector<PoseWithCurvature>& points,
+    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+    units::meters_per_second_t startVelocity,
+    units::meters_per_second_t endVelocity,
+    units::meters_per_second_t maxVelocity,
+    units::meters_per_second_squared_t maxAcceleration, bool reversed) {
+  std::vector<ConstrainedState> constrainedStates(points.size());
+
+  ConstrainedState predecessor{points.front(), 0_m, startVelocity,
+                               -maxAcceleration, maxAcceleration};
+
+  constrainedStates[0] = predecessor;
+
+  // Forward pass
+  for (unsigned int i = 0; i < points.size(); i++) {
+    auto& constrainedState = constrainedStates[i];
+    constrainedState.pose = points[i];
+
+    // Begin constraining based on predecessor
+    units::meter_t ds = constrainedState.pose.first.Translation().Distance(
+        predecessor.pose.first.Translation());
+    constrainedState.distance = ds + predecessor.distance;
+
+    // We may need to iterate to find the maximum end velocity and common
+    // acceleration, since acceleration limits may be a function of velocity.
+    while (true) {
+      // Enforce global max velocity and max reachable velocity by global
+      // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+
+      constrainedState.maxVelocity = units::math::min(
+          maxVelocity,
+          units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
+                            predecessor.maxAcceleration * ds * 2.0));
+
+      constrainedState.minAcceleration = -maxAcceleration;
+      constrainedState.maxAcceleration = maxAcceleration;
+
+      // At this point, the constrained state is fully constructed apart from
+      // all the custom-defined user constraints.
+      for (const auto& constraint : constraints) {
+        constrainedState.maxVelocity = units::math::min(
+            constrainedState.maxVelocity,
+            constraint->MaxVelocity(constrainedState.pose.first,
+                                    constrainedState.pose.second,
+                                    constrainedState.maxVelocity));
+      }
+
+      // Now enforce all acceleration limits.
+      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+      if (ds.to<double>() < kEpsilon) break;
+
+      // If the actual acceleration for this state is higher than the max
+      // acceleration that we applied, then we need to reduce the max
+      // acceleration of the predecessor and try again.
+      units::meters_per_second_squared_t actualAcceleration =
+          (constrainedState.maxVelocity * constrainedState.maxVelocity -
+           predecessor.maxVelocity * predecessor.maxVelocity) /
+          (ds * 2.0);
+
+      // If we violate the max acceleration constraint, let's modify the
+      // predecessor.
+      if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) {
+        predecessor.maxAcceleration = constrainedState.maxAcceleration;
+      } else {
+        // Constrain the predecessor's max acceleration to the current
+        // acceleration.
+        if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) {
+          predecessor.maxAcceleration = actualAcceleration;
+        }
+        // If the actual acceleration is less than the predecessor's min
+        // acceleration, it will be repaired in the backward pass.
+        break;
+      }
+    }
+    predecessor = constrainedState;
+  }
+
+  ConstrainedState successor{points.back(), constrainedStates.back().distance,
+                             endVelocity, -maxAcceleration, maxAcceleration};
+
+  // Backward pass
+  for (int i = points.size() - 1; i >= 0; i--) {
+    auto& constrainedState = constrainedStates[i];
+    units::meter_t ds =
+        constrainedState.distance - successor.distance;  // negative
+
+    while (true) {
+      // Enforce max velocity limit (reverse)
+      // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+      units::meters_per_second_t newMaxVelocity =
+          units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
+                            successor.minAcceleration * ds * 2.0);
+
+      // No more limits to impose! This state can be finalized.
+      if (newMaxVelocity >= constrainedState.maxVelocity) break;
+
+      constrainedState.maxVelocity = newMaxVelocity;
+
+      // Check all acceleration constraints with the new max velocity.
+      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+      if (ds.to<double>() > -kEpsilon) break;
+
+      // If the actual acceleration for this state is lower than the min
+      // acceleration, then we need to lower the min acceleration of the
+      // successor and try again.
+      units::meters_per_second_squared_t actualAcceleration =
+          (constrainedState.maxVelocity * constrainedState.maxVelocity -
+           successor.maxVelocity * successor.maxVelocity) /
+          (ds * 2.0);
+      if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) {
+        successor.minAcceleration = constrainedState.minAcceleration;
+      } else {
+        successor.minAcceleration = actualAcceleration;
+        break;
+      }
+    }
+    successor = constrainedState;
+  }
+
+  // Now we can integrate the constrained states forward in time to obtain our
+  // trajectory states.
+
+  std::vector<Trajectory::State> states(points.size());
+  units::second_t t = 0_s;
+  units::meter_t s = 0_m;
+  units::meters_per_second_t v = 0_mps;
+
+  for (unsigned int i = 0; i < constrainedStates.size(); i++) {
+    auto state = constrainedStates[i];
+
+    // Calculate the change in position between the current state and the
+    // previous state.
+    units::meter_t ds = state.distance - s;
+
+    // Calculate the acceleration between the current state and the previous
+    // state.
+    units::meters_per_second_squared_t accel =
+        (state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
+
+    // Calculate dt.
+    units::second_t dt = 0_s;
+    if (i > 0) {
+      states.at(i - 1).acceleration = reversed ? -accel : accel;
+      if (units::math::abs(accel) > 1E-6_mps_sq) {
+        // v_f = v_0 + a * t
+        dt = (state.maxVelocity - v) / accel;
+      } else if (units::math::abs(v) > 1E-6_mps) {
+        // delta_x = v * t
+        dt = ds / v;
+      } else {
+        throw std::runtime_error("Something went wrong at iteration " +
+                                 std::to_string(i) +
+                                 " of time parameterization.");
+      }
+    }
+
+    v = state.maxVelocity;
+    s = state.distance;
+
+    t += dt;
+
+    states[i] = {t, reversed ? -v : v, reversed ? -accel : accel,
+                 state.pose.first, state.pose.second};
+  }
+
+  return Trajectory(states);
+}
+
+void TrajectoryParameterizer::EnforceAccelerationLimits(
+    bool reverse,
+    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+    ConstrainedState* state) {
+  for (auto&& constraint : constraints) {
+    double factor = reverse ? -1.0 : 1.0;
+
+    auto minMaxAccel = constraint->MinMaxAcceleration(
+        state->pose.first, state->pose.second, state->maxVelocity * factor);
+
+    if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
+      throw std::runtime_error(
+          "The constraint's min acceleration was greater than its max "
+          "acceleration. To debug this, remove all constraints from the config "
+          "and add each one individually. If the offending constraint was "
+          "packaged with WPILib, please file a bug report.");
+    }
+
+    state->minAcceleration = units::math::max(
+        state->minAcceleration,
+        reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
+
+    state->maxAcceleration = units::math::min(
+        state->maxAcceleration,
+        reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
+  }
+}

wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
new file mode 100644
index 0000000..0ae43f5
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryUtil.h"
+
+#include <system_error>
+
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
+                                      const wpi::Twine& path) {
+  std::error_code error_code;
+
+  wpi::SmallString<128> buf;
+  wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+  if (error_code) {
+    throw std::runtime_error(("Cannot open file: " + path).str());
+  }
+
+  wpi::json json = trajectory.States();
+  output << json;
+  output.flush();
+}
+
+Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+  std::error_code error_code;
+
+  wpi::SmallString<128> buf;
+  wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+  if (error_code) {
+    throw std::runtime_error(("Cannot open file: " + path).str());
+  }
+
+  wpi::json json;
+  input >> json;
+
+  return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
+
+std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
+  wpi::json json = trajectory.States();
+  return json.dump();
+}
+
+Trajectory TrajectoryUtil::DeserializeTrajectory(
+    const wpi::StringRef json_str) {
+  wpi::json json = wpi::json::parse(json_str);
+  return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}

wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
new file mode 100644
index 0000000..f04b9d6
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
+    units::meters_per_second_squared_t maxCentripetalAcceleration)
+    : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
+
+units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  // ac = v^2 / r
+  // k (curvature) = 1 / r
+
+  // therefore, ac = v^2 * k
+  // ac / k = v^2
+  // v = std::sqrt(ac / k)
+
+  // We have to multiply by 1_rad here to get the units to cancel out nicely.
+  // The units library defines a unit for radians although it is technically
+  // unitless.
+  return units::math::sqrt(m_maxCentripetalAcceleration /
+                           units::math::abs(curvature) * 1_rad);
+}
+
+TrajectoryConstraint::MinMax
+CentripetalAccelerationConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  // The acceleration of the robot has no impact on the centripetal acceleration
+  // of the robot.
+  return {};
+}

wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
new file mode 100644
index 0000000..c3380e5
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
+    const DifferentialDriveKinematics& kinematics,
+    units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
+  wheelSpeeds.Normalize(m_maxSpeed);
+
+  return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}

wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
new file mode 100644
index 0000000..f12ce75
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+
+#include <algorithm>
+#include <limits>
+
+#include <wpi/MathExtras.h>
+
+#include "units/math.h"
+
+using namespace frc;
+
+DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
+    const SimpleMotorFeedforward<units::meter>& feedforward,
+    const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage)
+    : m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_maxVoltage(maxVoltage) {}
+
+units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  return units::meters_per_second_t(std::numeric_limits<double>::max());
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveVoltageConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
+
+  auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
+  auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+
+  // Calculate maximum/minimum possible accelerations from motor dynamics
+  // and max/min wheel speeds
+  auto maxWheelAcceleration =
+      m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+  auto minWheelAcceleration =
+      m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+  // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
+  // increased by half of the trackwidth T.  Inner wheel has radius decreased
+  // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
+  // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 +
+  // |curvature|T/2). Inner wheel is similar.
+
+  // sgn(speed) term added to correctly account for which wheel is on
+  // outside of turn:
+  // If moving forward, max acceleration constraint corresponds to wheel on
+  // outside of turn If moving backward, max acceleration constraint corresponds
+  // to wheel on inside of turn
+
+  // When velocity is zero, then wheel velocities are uniformly zero (robot
+  // cannot be turning on its center) - we have to treat this as a special case,
+  // as it breaks the signum function.  Both max and min acceleration are
+  // *reduced in magnitude* in this case.
+
+  units::meters_per_second_squared_t maxChassisAcceleration;
+  units::meters_per_second_squared_t minChassisAcceleration;
+
+  if (speed == 0_mps) {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+  } else {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+  }
+
+  // When turning about a point inside of the wheelbase (i.e. radius less than
+  // half the trackwidth), the inner wheel's direction changes, but the
+  // magnitude remains the same.  The formula above changes sign for the inner
+  // wheel when this happens. We can accurately account for this by simply
+  // negating the inner wheel.
+
+  if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
+    if (speed > 0_mps) {
+      minChassisAcceleration = -minChassisAcceleration;
+    } else if (speed < 0_mps) {
+      maxChassisAcceleration = -maxChassisAcceleration;
+    }
+  }
+
+  return {minChassisAcceleration, maxChassisAcceleration};
+}

wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp

diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
new file mode 100644
index 0000000..7d95803
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
+    const MecanumDriveKinematics& kinematics,
+    units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  auto xVelocity = velocity * pose.Rotation().Cos();
+  auto yVelocity = velocity * pose.Rotation().Sin();
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
+  wheelSpeeds.Normalize(m_maxSpeed);
+
+  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+TrajectoryConstraint::MinMax
+MecanumDriveKinematicsConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}

wpimath/src/main/native/eigeninclude/Eigen/Cholesky

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Cholesky b/wpimath/src/main/native/eigeninclude/Eigen/Cholesky
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/Cholesky
rename to wpimath/src/main/native/eigeninclude/Eigen/Cholesky

wpimath/src/main/native/eigeninclude/Eigen/Core

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Core b/wpimath/src/main/native/eigeninclude/Eigen/Core
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/Core
rename to wpimath/src/main/native/eigeninclude/Eigen/Core

wpimath/src/main/native/eigeninclude/Eigen/Eigen

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Eigen b/wpimath/src/main/native/eigeninclude/Eigen/Eigen
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/Eigen
rename to wpimath/src/main/native/eigeninclude/Eigen/Eigen

wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Eigenvalues b/wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/Eigenvalues
rename to wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues

wpimath/src/main/native/eigeninclude/Eigen/Householder

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Householder b/wpimath/src/main/native/eigeninclude/Eigen/Householder
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/Householder
rename to wpimath/src/main/native/eigeninclude/Eigen/Householder

wpimath/src/main/native/eigeninclude/Eigen/Jacobi

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/Jacobi b/wpimath/src/main/native/eigeninclude/Eigen/Jacobi
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/Jacobi
rename to wpimath/src/main/native/eigeninclude/Eigen/Jacobi

wpimath/src/main/native/eigeninclude/Eigen/LU

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/LU b/wpimath/src/main/native/eigeninclude/Eigen/LU
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/LU
rename to wpimath/src/main/native/eigeninclude/Eigen/LU

wpimath/src/main/native/eigeninclude/Eigen/QR

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/QR b/wpimath/src/main/native/eigeninclude/Eigen/QR
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/QR
rename to wpimath/src/main/native/eigeninclude/Eigen/QR

wpimath/src/main/native/eigeninclude/Eigen/SVD

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/SVD b/wpimath/src/main/native/eigeninclude/Eigen/SVD
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/SVD
rename to wpimath/src/main/native/eigeninclude/Eigen/SVD

wpimath/src/main/native/eigeninclude/Eigen/StdDeque

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/StdDeque b/wpimath/src/main/native/eigeninclude/Eigen/StdDeque
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/StdDeque
rename to wpimath/src/main/native/eigeninclude/Eigen/StdDeque

wpimath/src/main/native/eigeninclude/Eigen/StdList

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/StdList b/wpimath/src/main/native/eigeninclude/Eigen/StdList
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/StdList
rename to wpimath/src/main/native/eigeninclude/Eigen/StdList

wpimath/src/main/native/eigeninclude/Eigen/StdVector

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/StdVector b/wpimath/src/main/native/eigeninclude/Eigen/StdVector
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/StdVector
rename to wpimath/src/main/native/eigeninclude/Eigen/StdVector

wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h

wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Array.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Array.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
new file mode 100644
index 0000000..33f644e
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
@@ -0,0 +1,226 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARRAYBASE_H
+#define EIGEN_ARRAYBASE_H
+
+namespace Eigen { 
+
+template<typename ExpressionType> class MatrixWrapper;
+
+/** \class ArrayBase
+  * \ingroup Core_Module
+  *
+  * \brief Base class for all 1D and 2D array, and related expressions
+  *
+  * An array is similar to a dense vector or matrix. While matrices are mathematical
+  * objects with well defined linear algebra operators, an array is just a collection
+  * of scalar values arranged in a one or two dimensionnal fashion. As the main consequence,
+  * all operations applied to an array are performed coefficient wise. Furthermore,
+  * arrays support scalar math functions of the c++ standard library (e.g., std::sin(x)), and convenient
+  * constructors allowing to easily write generic code working for both scalar values
+  * and arrays.
+  *
+  * This class is the base that is inherited by all array expression types.
+  *
+  * \tparam Derived is the derived type, e.g., an array or an expression type.
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAYBASE_PLUGIN.
+  *
+  * \sa class MatrixBase, \ref TopicClassHierarchy
+  */
+template<typename Derived> class ArrayBase
+  : public DenseBase<Derived>
+{
+  public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** The base class for a given storage type. */
+    typedef ArrayBase StorageBaseType;
+
+    typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl;
+
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    typedef DenseBase<Derived> Base;
+    using Base::RowsAtCompileTime;
+    using Base::ColsAtCompileTime;
+    using Base::SizeAtCompileTime;
+    using Base::MaxRowsAtCompileTime;
+    using Base::MaxColsAtCompileTime;
+    using Base::MaxSizeAtCompileTime;
+    using Base::IsVectorAtCompileTime;
+    using Base::Flags;
+    
+    using Base::derived;
+    using Base::const_cast_derived;
+    using Base::rows;
+    using Base::cols;
+    using Base::size;
+    using Base::coeff;
+    using Base::coeffRef;
+    using Base::lazyAssign;
+    using Base::operator=;
+    using Base::operator+=;
+    using Base::operator-=;
+    using Base::operator*=;
+    using Base::operator/=;
+
+    typedef typename Base::CoeffReturnType CoeffReturnType;
+
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typedef typename Base::PlainObject PlainObject;
+
+    /** \internal Represents a matrix with all coefficients equal to one another*/
+    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::ArrayBase
+#define EIGEN_DOC_UNARY_ADDONS(X,Y)
+#   include "../plugins/CommonCwiseUnaryOps.h"
+#   include "../plugins/MatrixCwiseUnaryOps.h"
+#   include "../plugins/ArrayCwiseUnaryOps.h"
+#   include "../plugins/CommonCwiseBinaryOps.h"
+#   include "../plugins/MatrixCwiseBinaryOps.h"
+#   include "../plugins/ArrayCwiseBinaryOps.h"
+#   ifdef EIGEN_ARRAYBASE_PLUGIN
+#     include EIGEN_ARRAYBASE_PLUGIN
+#   endif
+#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
+#undef EIGEN_DOC_UNARY_ADDONS
+
+    /** Special case of the template operator=, in order to prevent the compiler
+      * from generating a default operator= (issue hit with g++ 4.1)
+      */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const ArrayBase& other)
+    {
+      internal::call_assignment(derived(), other.derived());
+      return derived();
+    }
+    
+    /** Set all the entries to \a value.
+      * \sa DenseBase::setConstant(), DenseBase::fill() */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const Scalar &value)
+    { Base::setConstant(value); return derived(); }
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator+=(const Scalar& scalar);
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator-=(const Scalar& scalar);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator+=(const ArrayBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator-=(const ArrayBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator*=(const ArrayBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator/=(const ArrayBase<OtherDerived>& other);
+
+  public:
+    EIGEN_DEVICE_FUNC
+    ArrayBase<Derived>& array() { return *this; }
+    EIGEN_DEVICE_FUNC
+    const ArrayBase<Derived>& array() const { return *this; }
+
+    /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array
+      * \sa MatrixBase::array() */
+    EIGEN_DEVICE_FUNC
+    MatrixWrapper<Derived> matrix() { return MatrixWrapper<Derived>(derived()); }
+    EIGEN_DEVICE_FUNC
+    const MatrixWrapper<const Derived> matrix() const { return MatrixWrapper<const Derived>(derived()); }
+
+//     template<typename Dest>
+//     inline void evalTo(Dest& dst) const { dst = matrix(); }
+
+  protected:
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(ArrayBase)
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(ArrayBase)
+
+  private:
+    explicit ArrayBase(Index);
+    ArrayBase(Index,Index);
+    template<typename OtherDerived> explicit ArrayBase(const ArrayBase<OtherDerived>&);
+  protected:
+    // mixing arrays and matrices is not legal
+    template<typename OtherDerived> Derived& operator+=(const MatrixBase<OtherDerived>& )
+    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+    // mixing arrays and matrices is not legal
+    template<typename OtherDerived> Derived& operator-=(const MatrixBase<OtherDerived>& )
+    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+};
+
+/** replaces \c *this by \c *this - \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator-=(const ArrayBase<OtherDerived> &other)
+{
+  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+/** replaces \c *this by \c *this + \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator+=(const ArrayBase<OtherDerived>& other)
+{
+  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+/** replaces \c *this by \c *this * \a other coefficient wise.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator*=(const ArrayBase<OtherDerived>& other)
+{
+  call_assignment(derived(), other.derived(), internal::mul_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+/** replaces \c *this by \c *this / \a other coefficient wise.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
+ArrayBase<Derived>::operator/=(const ArrayBase<OtherDerived>& other)
+{
+  call_assignment(derived(), other.derived(), internal::div_assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_ARRAYBASE_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Block.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Block.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/ConditionEstimator.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseTernaryOp.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
new file mode 100644
index 0000000..5a30fa8
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
@@ -0,0 +1,130 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CWISE_UNARY_VIEW_H
+#define EIGEN_CWISE_UNARY_VIEW_H
+
+namespace Eigen {
+
+namespace internal {
+template<typename ViewOp, typename MatrixType>
+struct traits<CwiseUnaryView<ViewOp, MatrixType> >
+ : traits<MatrixType>
+{
+  typedef typename result_of<
+                     ViewOp(const typename traits<MatrixType>::Scalar&)
+                   >::type Scalar;
+  typedef typename MatrixType::Nested MatrixTypeNested;
+  typedef typename remove_all<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
+    Flags = traits<_MatrixTypeNested>::Flags & (RowMajorBit | FlagsLvalueBit | DirectAccessBit), // FIXME DirectAccessBit should not be handled by expressions
+    MatrixTypeInnerStride =  inner_stride_at_compile_time<MatrixType>::ret,
+    // need to cast the sizeof's from size_t to int explicitly, otherwise:
+    // "error: no integral type can represent all of the enumerator values
+    InnerStrideAtCompileTime = MatrixTypeInnerStride == Dynamic
+                             ? int(Dynamic)
+                             : int(MatrixTypeInnerStride) * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar)),
+    OuterStrideAtCompileTime = outer_stride_at_compile_time<MatrixType>::ret == Dynamic
+                             ? int(Dynamic)
+                             : outer_stride_at_compile_time<MatrixType>::ret * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar))
+  };
+};
+}
+
+template<typename ViewOp, typename MatrixType, typename StorageKind>
+class CwiseUnaryViewImpl;
+
+/** \class CwiseUnaryView
+  * \ingroup Core_Module
+  *
+  * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector
+  *
+  * \tparam ViewOp template functor implementing the view
+  * \tparam MatrixType the type of the matrix we are applying the unary operator
+  *
+  * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector.
+  * It is the return type of real() and imag(), and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp
+  */
+template<typename ViewOp, typename MatrixType>
+class CwiseUnaryView : public CwiseUnaryViewImpl<ViewOp, MatrixType, typename internal::traits<MatrixType>::StorageKind>
+{
+  public:
+
+    typedef typename CwiseUnaryViewImpl<ViewOp, MatrixType,typename internal::traits<MatrixType>::StorageKind>::Base Base;
+    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryView)
+    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+
+    explicit inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp())
+      : m_matrix(mat), m_functor(func) {}
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView)
+
+    EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); }
+    EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); }
+
+    /** \returns the functor representing unary operation */
+    const ViewOp& functor() const { return m_functor; }
+
+    /** \returns the nested expression */
+    const typename internal::remove_all<MatrixTypeNested>::type&
+    nestedExpression() const { return m_matrix; }
+
+    /** \returns the nested expression */
+    typename internal::remove_reference<MatrixTypeNested>::type&
+    nestedExpression() { return m_matrix.const_cast_derived(); }
+
+  protected:
+    MatrixTypeNested m_matrix;
+    ViewOp m_functor;
+};
+
+// Generic API dispatcher
+template<typename ViewOp, typename XprType, typename StorageKind>
+class CwiseUnaryViewImpl
+  : public internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type
+{
+public:
+  typedef typename internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type Base;
+};
+
+template<typename ViewOp, typename MatrixType>
+class CwiseUnaryViewImpl<ViewOp,MatrixType,Dense>
+  : public internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type
+{
+  public:
+
+    typedef CwiseUnaryView<ViewOp, MatrixType> Derived;
+    typedef typename internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type Base;
+
+    EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl)
+    
+    EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); }
+    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); }
+
+    EIGEN_DEVICE_FUNC inline Index innerStride() const
+    {
+      return derived().nestedExpression().innerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
+    }
+
+    EIGEN_DEVICE_FUNC inline Index outerStride() const
+    {
+      return derived().nestedExpression().outerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
+    }
+  protected:
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(CwiseUnaryViewImpl)
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_CWISE_UNARY_VIEW_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
new file mode 100644
index 0000000..c27a8a8
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
@@ -0,0 +1,612 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2007-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_DENSEBASE_H
+#define EIGEN_DENSEBASE_H
+
+namespace Eigen {
+
+namespace internal {
+  
+// The index type defined by EIGEN_DEFAULT_DENSE_INDEX_TYPE must be a signed type.
+// This dummy function simply aims at checking that at compile time.
+static inline void check_DenseIndex_is_signed() {
+  EIGEN_STATIC_ASSERT(NumTraits<DenseIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE); 
+}
+
+} // end namespace internal
+  
+/** \class DenseBase
+  * \ingroup Core_Module
+  *
+  * \brief Base class for all dense matrices, vectors, and arrays
+  *
+  * This class is the base that is inherited by all dense objects (matrix, vector, arrays,
+  * and related expression types). The common Eigen API for dense objects is contained in this class.
+  *
+  * \tparam Derived is the derived type, e.g., a matrix type or an expression.
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_DENSEBASE_PLUGIN.
+  *
+  * \sa \blank \ref TopicClassHierarchy
+  */
+template<typename Derived> class DenseBase
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  : public DenseCoeffsBase<Derived>
+#else
+  : public DenseCoeffsBase<Derived,DirectWriteAccessors>
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+{
+  public:
+
+    /** Inner iterator type to iterate over the coefficients of a row or column.
+      * \sa class InnerIterator
+      */
+    typedef Eigen::InnerIterator<Derived> InnerIterator;
+
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+
+    /**
+      * \brief The type used to store indices
+      * \details This typedef is relevant for types that store multiple indices such as
+      *          PermutationMatrix or Transpositions, otherwise it defaults to Eigen::Index
+      * \sa \blank \ref TopicPreprocessorDirectives, Eigen::Index, SparseMatrixBase.
+     */
+    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+
+    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc. */
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    
+    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
+      *
+      * It is an alias for the Scalar type */
+    typedef Scalar value_type;
+    
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef DenseCoeffsBase<Derived> Base;
+
+    using Base::derived;
+    using Base::const_cast_derived;
+    using Base::rows;
+    using Base::cols;
+    using Base::size;
+    using Base::rowIndexByOuterInner;
+    using Base::colIndexByOuterInner;
+    using Base::coeff;
+    using Base::coeffByOuterInner;
+    using Base::operator();
+    using Base::operator[];
+    using Base::x;
+    using Base::y;
+    using Base::z;
+    using Base::w;
+    using Base::stride;
+    using Base::innerStride;
+    using Base::outerStride;
+    using Base::rowStride;
+    using Base::colStride;
+    typedef typename Base::CoeffReturnType CoeffReturnType;
+
+    enum {
+
+      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+        /**< The number of rows at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+
+      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+        /**< The number of columns at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+
+
+      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
+                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
+        /**< This is equal to the number of coefficients, i.e. the number of
+          * rows times the number of columns, or to \a Dynamic if this is not
+          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+
+      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
+        /**< This value is equal to the maximum possible number of rows that this expression
+          * might have. If this expression might have an arbitrarily high number of rows,
+          * this value is set to \a Dynamic.
+          *
+          * This value is useful to know when evaluating an expression, in order to determine
+          * whether it is possible to avoid doing a dynamic memory allocation.
+          *
+          * \sa RowsAtCompileTime, MaxColsAtCompileTime, MaxSizeAtCompileTime
+          */
+
+      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
+        /**< This value is equal to the maximum possible number of columns that this expression
+          * might have. If this expression might have an arbitrarily high number of columns,
+          * this value is set to \a Dynamic.
+          *
+          * This value is useful to know when evaluating an expression, in order to determine
+          * whether it is possible to avoid doing a dynamic memory allocation.
+          *
+          * \sa ColsAtCompileTime, MaxRowsAtCompileTime, MaxSizeAtCompileTime
+          */
+
+      MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
+                                                      internal::traits<Derived>::MaxColsAtCompileTime>::ret),
+        /**< This value is equal to the maximum possible number of coefficients that this expression
+          * might have. If this expression might have an arbitrarily high number of coefficients,
+          * this value is set to \a Dynamic.
+          *
+          * This value is useful to know when evaluating an expression, in order to determine
+          * whether it is possible to avoid doing a dynamic memory allocation.
+          *
+          * \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime
+          */
+
+      IsVectorAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime == 1
+                           || internal::traits<Derived>::MaxColsAtCompileTime == 1,
+        /**< This is set to true if either the number of rows or the number of
+          * columns is known at compile-time to be equal to 1. Indeed, in that case,
+          * we are dealing with a column-vector (if there is only one column) or with
+          * a row-vector (if there is only one row). */
+
+      Flags = internal::traits<Derived>::Flags,
+        /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
+          * constructed from this one. See the \ref flags "list of flags".
+          */
+
+      IsRowMajor = int(Flags) & RowMajorBit, /**< True if this expression has row-major storage order. */
+
+      InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
+                             : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+
+      InnerStrideAtCompileTime = internal::inner_stride_at_compile_time<Derived>::ret,
+      OuterStrideAtCompileTime = internal::outer_stride_at_compile_time<Derived>::ret
+    };
+    
+    typedef typename internal::find_best_packet<Scalar,SizeAtCompileTime>::type PacketScalar;
+
+    enum { IsPlainObjectBase = 0 };
+    
+    /** The plain matrix type corresponding to this expression.
+      * \sa PlainObject */
+    typedef Matrix<typename internal::traits<Derived>::Scalar,
+                internal::traits<Derived>::RowsAtCompileTime,
+                internal::traits<Derived>::ColsAtCompileTime,
+                AutoAlign | (internal::traits<Derived>::Flags&RowMajorBit ? RowMajor : ColMajor),
+                internal::traits<Derived>::MaxRowsAtCompileTime,
+                internal::traits<Derived>::MaxColsAtCompileTime
+          > PlainMatrix;
+    
+    /** The plain array type corresponding to this expression.
+      * \sa PlainObject */
+    typedef Array<typename internal::traits<Derived>::Scalar,
+                internal::traits<Derived>::RowsAtCompileTime,
+                internal::traits<Derived>::ColsAtCompileTime,
+                AutoAlign | (internal::traits<Derived>::Flags&RowMajorBit ? RowMajor : ColMajor),
+                internal::traits<Derived>::MaxRowsAtCompileTime,
+                internal::traits<Derived>::MaxColsAtCompileTime
+          > PlainArray;
+
+    /** \brief The plain matrix or array type corresponding to this expression.
+      *
+      * This is not necessarily exactly the return type of eval(). In the case of plain matrices,
+      * the return type of eval() is a const reference to a matrix, not a matrix! It is however guaranteed
+      * that the return type of eval() is either PlainObject or const PlainObject&.
+      */
+    typedef typename internal::conditional<internal::is_same<typename internal::traits<Derived>::XprKind,MatrixXpr >::value,
+                                 PlainMatrix, PlainArray>::type PlainObject;
+
+    /** \returns the number of nonzero coefficients which is in practice the number
+      * of stored coefficients. */
+    EIGEN_DEVICE_FUNC
+    inline Index nonZeros() const { return size(); }
+
+    /** \returns the outer size.
+      *
+      * \note For a vector, this returns just 1. For a matrix (non-vector), this is the major dimension
+      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of columns for a
+      * column-major matrix, and the number of rows for a row-major matrix. */
+    EIGEN_DEVICE_FUNC
+    Index outerSize() const
+    {
+      return IsVectorAtCompileTime ? 1
+           : int(IsRowMajor) ? this->rows() : this->cols();
+    }
+
+    /** \returns the inner size.
+      *
+      * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension
+      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a 
+      * column-major matrix, and the number of columns for a row-major matrix. */
+    EIGEN_DEVICE_FUNC
+    Index innerSize() const
+    {
+      return IsVectorAtCompileTime ? this->size()
+           : int(IsRowMajor) ? this->cols() : this->rows();
+    }
+
+    /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
+      * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does
+      * nothing else.
+      */
+    EIGEN_DEVICE_FUNC
+    void resize(Index newSize)
+    {
+      EIGEN_ONLY_USED_FOR_DEBUG(newSize);
+      eigen_assert(newSize == this->size()
+                && "DenseBase::resize() does not actually allow to resize.");
+    }
+    /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
+      * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does
+      * nothing else.
+      */
+    EIGEN_DEVICE_FUNC
+    void resize(Index rows, Index cols)
+    {
+      EIGEN_ONLY_USED_FOR_DEBUG(rows);
+      EIGEN_ONLY_USED_FOR_DEBUG(cols);
+      eigen_assert(rows == this->rows() && cols == this->cols()
+                && "DenseBase::resize() does not actually allow to resize.");
+    }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal Represents a matrix with all coefficients equal to one another*/
+    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
+    /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */
+    typedef CwiseNullaryOp<internal::linspaced_op<Scalar,PacketScalar>,PlainObject> SequentialLinSpacedReturnType;
+    /** \internal Represents a vector with linearly spaced coefficients that allows random access. */
+    typedef CwiseNullaryOp<internal::linspaced_op<Scalar,PacketScalar>,PlainObject> RandomAccessLinSpacedReturnType;
+    /** \internal the return type of MatrixBase::eigenvalues() */
+    typedef Matrix<typename NumTraits<typename internal::traits<Derived>::Scalar>::Real, internal::traits<Derived>::ColsAtCompileTime, 1> EigenvaluesReturnType;
+
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    /** Copies \a other into *this. \returns a reference to *this. */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const DenseBase<OtherDerived>& other);
+
+    /** Special case of the template operator=, in order to prevent the compiler
+      * from generating a default operator= (issue hit with g++ 4.1)
+      */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const DenseBase& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    Derived& operator=(const EigenBase<OtherDerived> &other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    Derived& operator+=(const EigenBase<OtherDerived> &other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    Derived& operator-=(const EigenBase<OtherDerived> &other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    Derived& operator=(const ReturnByValue<OtherDerived>& func);
+
+    /** \internal
+      * Copies \a other into *this without evaluating other. \returns a reference to *this.
+      * \deprecated */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    Derived& lazyAssign(const DenseBase<OtherDerived>& other);
+
+    EIGEN_DEVICE_FUNC
+    CommaInitializer<Derived> operator<< (const Scalar& s);
+
+    /** \deprecated it now returns \c *this */
+    template<unsigned int Added,unsigned int Removed>
+    EIGEN_DEPRECATED
+    const Derived& flagged() const
+    { return derived(); }
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    CommaInitializer<Derived> operator<< (const DenseBase<OtherDerived>& other);
+
+    typedef Transpose<Derived> TransposeReturnType;
+    EIGEN_DEVICE_FUNC
+    TransposeReturnType transpose();
+    typedef typename internal::add_const<Transpose<const Derived> >::type ConstTransposeReturnType;
+    EIGEN_DEVICE_FUNC
+    ConstTransposeReturnType transpose() const;
+    EIGEN_DEVICE_FUNC
+    void transposeInPlace();
+
+    EIGEN_DEVICE_FUNC static const ConstantReturnType
+    Constant(Index rows, Index cols, const Scalar& value);
+    EIGEN_DEVICE_FUNC static const ConstantReturnType
+    Constant(Index size, const Scalar& value);
+    EIGEN_DEVICE_FUNC static const ConstantReturnType
+    Constant(const Scalar& value);
+
+    EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType
+    LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high);
+    EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
+    LinSpaced(Index size, const Scalar& low, const Scalar& high);
+    EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType
+    LinSpaced(Sequential_t, const Scalar& low, const Scalar& high);
+    EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
+    LinSpaced(const Scalar& low, const Scalar& high);
+
+    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
+    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
+    NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func);
+    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
+    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
+    NullaryExpr(Index size, const CustomNullaryOp& func);
+    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
+    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
+    NullaryExpr(const CustomNullaryOp& func);
+
+    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index rows, Index cols);
+    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index size);
+    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero();
+    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index rows, Index cols);
+    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index size);
+    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones();
+
+    EIGEN_DEVICE_FUNC void fill(const Scalar& value);
+    EIGEN_DEVICE_FUNC Derived& setConstant(const Scalar& value);
+    EIGEN_DEVICE_FUNC Derived& setLinSpaced(Index size, const Scalar& low, const Scalar& high);
+    EIGEN_DEVICE_FUNC Derived& setLinSpaced(const Scalar& low, const Scalar& high);
+    EIGEN_DEVICE_FUNC Derived& setZero();
+    EIGEN_DEVICE_FUNC Derived& setOnes();
+    EIGEN_DEVICE_FUNC Derived& setRandom();
+
+    template<typename OtherDerived> EIGEN_DEVICE_FUNC
+    bool isApprox(const DenseBase<OtherDerived>& other,
+                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    EIGEN_DEVICE_FUNC 
+    bool isMuchSmallerThan(const RealScalar& other,
+                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    template<typename OtherDerived> EIGEN_DEVICE_FUNC
+    bool isMuchSmallerThan(const DenseBase<OtherDerived>& other,
+                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
+    EIGEN_DEVICE_FUNC bool isApproxToConstant(const Scalar& value, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    EIGEN_DEVICE_FUNC bool isConstant(const Scalar& value, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    EIGEN_DEVICE_FUNC bool isZero(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    EIGEN_DEVICE_FUNC bool isOnes(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    
+    inline bool hasNaN() const;
+    inline bool allFinite() const;
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator*=(const Scalar& other);
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator/=(const Scalar& other);
+
+    typedef typename internal::add_const_on_value_type<typename internal::eval<Derived>::type>::type EvalReturnType;
+    /** \returns the matrix or vector obtained by evaluating this expression.
+      *
+      * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
+      * a const reference, in order to avoid a useless copy.
+      * 
+      * \warning Be carefull with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page \endlink.
+      */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE EvalReturnType eval() const
+    {
+      // Even though MSVC does not honor strong inlining when the return type
+      // is a dynamic matrix, we desperately need strong inlining for fixed
+      // size types on MSVC.
+      return typename internal::eval<Derived>::type(derived());
+    }
+    
+    /** swaps *this with the expression \a other.
+      *
+      */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    void swap(const DenseBase<OtherDerived>& other)
+    {
+      EIGEN_STATIC_ASSERT(!OtherDerived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+      eigen_assert(rows()==other.rows() && cols()==other.cols());
+      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
+    }
+
+    /** swaps *this with the matrix or array \a other.
+      *
+      */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    void swap(PlainObjectBase<OtherDerived>& other)
+    {
+      eigen_assert(rows()==other.rows() && cols()==other.cols());
+      call_assignment(derived(), other.derived(), internal::swap_assign_op<Scalar>());
+    }
+
+    EIGEN_DEVICE_FUNC inline const NestByValue<Derived> nestByValue() const;
+    EIGEN_DEVICE_FUNC inline const ForceAlignedAccess<Derived> forceAlignedAccess() const;
+    EIGEN_DEVICE_FUNC inline ForceAlignedAccess<Derived> forceAlignedAccess();
+    template<bool Enable> EIGEN_DEVICE_FUNC
+    inline const typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type forceAlignedAccessIf() const;
+    template<bool Enable> EIGEN_DEVICE_FUNC
+    inline typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type forceAlignedAccessIf();
+
+    EIGEN_DEVICE_FUNC Scalar sum() const;
+    EIGEN_DEVICE_FUNC Scalar mean() const;
+    EIGEN_DEVICE_FUNC Scalar trace() const;
+
+    EIGEN_DEVICE_FUNC Scalar prod() const;
+
+    EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar minCoeff() const;
+    EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar maxCoeff() const;
+
+    template<typename IndexType> EIGEN_DEVICE_FUNC
+    typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const;
+    template<typename IndexType> EIGEN_DEVICE_FUNC
+    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const;
+    template<typename IndexType> EIGEN_DEVICE_FUNC
+    typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const;
+    template<typename IndexType> EIGEN_DEVICE_FUNC
+    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const;
+
+    template<typename BinaryOp>
+    EIGEN_DEVICE_FUNC
+    Scalar redux(const BinaryOp& func) const;
+
+    template<typename Visitor>
+    EIGEN_DEVICE_FUNC
+    void visit(Visitor& func) const;
+
+    /** \returns a WithFormat proxy object allowing to print a matrix the with given
+      * format \a fmt.
+      *
+      * See class IOFormat for some examples.
+      *
+      * \sa class IOFormat, class WithFormat
+      */
+    inline const WithFormat<Derived> format(const IOFormat& fmt) const
+    {
+      return WithFormat<Derived>(derived(), fmt);
+    }
+
+    /** \returns the unique coefficient of a 1x1 expression */
+    EIGEN_DEVICE_FUNC
+    CoeffReturnType value() const
+    {
+      EIGEN_STATIC_ASSERT_SIZE_1x1(Derived)
+      eigen_assert(this->rows() == 1 && this->cols() == 1);
+      return derived().coeff(0,0);
+    }
+
+    EIGEN_DEVICE_FUNC bool all() const;
+    EIGEN_DEVICE_FUNC bool any() const;
+    EIGEN_DEVICE_FUNC Index count() const;
+
+    typedef VectorwiseOp<Derived, Horizontal> RowwiseReturnType;
+    typedef const VectorwiseOp<const Derived, Horizontal> ConstRowwiseReturnType;
+    typedef VectorwiseOp<Derived, Vertical> ColwiseReturnType;
+    typedef const VectorwiseOp<const Derived, Vertical> ConstColwiseReturnType;
+
+    /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations
+    *
+    * Example: \include MatrixBase_rowwise.cpp
+    * Output: \verbinclude MatrixBase_rowwise.out
+    *
+    * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
+    */
+    //Code moved here due to a CUDA compiler bug
+    EIGEN_DEVICE_FUNC inline ConstRowwiseReturnType rowwise() const {
+      return ConstRowwiseReturnType(derived());
+    }
+    EIGEN_DEVICE_FUNC RowwiseReturnType rowwise();
+
+    /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations
+    *
+    * Example: \include MatrixBase_colwise.cpp
+    * Output: \verbinclude MatrixBase_colwise.out
+    *
+    * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
+    */
+    EIGEN_DEVICE_FUNC inline ConstColwiseReturnType colwise() const {
+      return ConstColwiseReturnType(derived());
+    }
+    EIGEN_DEVICE_FUNC ColwiseReturnType colwise();
+
+    typedef CwiseNullaryOp<internal::scalar_random_op<Scalar>,PlainObject> RandomReturnType;
+    static const RandomReturnType Random(Index rows, Index cols);
+    static const RandomReturnType Random(Index size);
+    static const RandomReturnType Random();
+
+    template<typename ThenDerived,typename ElseDerived>
+    const Select<Derived,ThenDerived,ElseDerived>
+    select(const DenseBase<ThenDerived>& thenMatrix,
+           const DenseBase<ElseDerived>& elseMatrix) const;
+
+    template<typename ThenDerived>
+    inline const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
+    select(const DenseBase<ThenDerived>& thenMatrix, const typename ThenDerived::Scalar& elseScalar) const;
+
+    template<typename ElseDerived>
+    inline const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
+    select(const typename ElseDerived::Scalar& thenScalar, const DenseBase<ElseDerived>& elseMatrix) const;
+
+    template<int p> RealScalar lpNorm() const;
+
+    template<int RowFactor, int ColFactor>
+    EIGEN_DEVICE_FUNC
+    const Replicate<Derived,RowFactor,ColFactor> replicate() const;
+    /**
+    * \return an expression of the replication of \c *this
+    *
+    * Example: \include MatrixBase_replicate_int_int.cpp
+    * Output: \verbinclude MatrixBase_replicate_int_int.out
+    *
+    * \sa VectorwiseOp::replicate(), DenseBase::replicate<int,int>(), class Replicate
+    */
+    //Code moved here due to a CUDA compiler bug
+    EIGEN_DEVICE_FUNC
+    const Replicate<Derived, Dynamic, Dynamic> replicate(Index rowFactor, Index colFactor) const
+    {
+      return Replicate<Derived, Dynamic, Dynamic>(derived(), rowFactor, colFactor);
+    }
+
+    typedef Reverse<Derived, BothDirections> ReverseReturnType;
+    typedef const Reverse<const Derived, BothDirections> ConstReverseReturnType;
+    EIGEN_DEVICE_FUNC ReverseReturnType reverse();
+    /** This is the const version of reverse(). */
+    //Code moved here due to a CUDA compiler bug
+    EIGEN_DEVICE_FUNC ConstReverseReturnType reverse() const
+    {
+      return ConstReverseReturnType(derived());
+    }
+    EIGEN_DEVICE_FUNC void reverseInPlace();
+
+#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::DenseBase
+#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
+#   include "../plugins/BlockMethods.h"
+#   ifdef EIGEN_DENSEBASE_PLUGIN
+#     include EIGEN_DENSEBASE_PLUGIN
+#   endif
+#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
+#undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
+#undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
+
+    // disable the use of evalTo for dense objects with a nice compilation error
+    template<typename Dest>
+    EIGEN_DEVICE_FUNC
+    inline void evalTo(Dest& ) const
+    {
+      EIGEN_STATIC_ASSERT((internal::is_same<Dest,void>::value),THE_EVAL_EVALTO_FUNCTION_SHOULD_NEVER_BE_CALLED_FOR_DENSE_OBJECTS);
+    }
+
+  protected:
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(DenseBase)
+    /** Default constructor. Do nothing. */
+    EIGEN_DEVICE_FUNC DenseBase()
+    {
+      /* Just checks for self-consistency of the flags.
+       * Only do it when debugging Eigen, as this borders on paranoia and could slow compilation down
+       */
+#ifdef EIGEN_INTERNAL_DEBUGGING
+      EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, int(IsRowMajor))
+                        && EIGEN_IMPLIES(MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1, int(!IsRowMajor))),
+                          INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION)
+#endif
+    }
+
+  private:
+    EIGEN_DEVICE_FUNC explicit DenseBase(int);
+    EIGEN_DEVICE_FUNC DenseBase(int,int);
+    template<typename OtherDerived> EIGEN_DEVICE_FUNC explicit DenseBase(const DenseBase<OtherDerived>&);
+};
+
+} // end namespace Eigen
+
+#endif // EIGEN_DENSEBASE_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Dot.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Dot.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/IO.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/IO.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Map.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Map.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
new file mode 100644
index 0000000..92c3b28
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
@@ -0,0 +1,308 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2007-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MAPBASE_H
+#define EIGEN_MAPBASE_H
+
+#define EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) \
+      EIGEN_STATIC_ASSERT((int(internal::evaluator<Derived>::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \
+                          YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT)
+
+namespace Eigen { 
+
+/** \ingroup Core_Module
+  *
+  * \brief Base class for dense Map and Block expression with direct access
+  *
+  * This base class provides the const low-level accessors (e.g. coeff, coeffRef) of dense
+  * Map and Block objects with direct access.
+  * Typical users do not have to directly deal with this class.
+  *
+  * This class can be extended by through the macro plugin \c EIGEN_MAPBASE_PLUGIN.
+  * See \link TopicCustomizing_Plugins customizing Eigen \endlink for details.
+  *
+  * The \c Derived class has to provide the following two methods describing the memory layout:
+  *  \code Index innerStride() const; \endcode
+  *  \code Index outerStride() const; \endcode
+  *
+  * \sa class Map, class Block
+  */
+template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
+  : public internal::dense_xpr_base<Derived>::type
+{
+  public:
+
+    typedef typename internal::dense_xpr_base<Derived>::type Base;
+    enum {
+      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+      InnerStrideAtCompileTime = internal::traits<Derived>::InnerStrideAtCompileTime,
+      SizeAtCompileTime = Base::SizeAtCompileTime
+    };
+
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef typename internal::conditional<
+                         bool(internal::is_lvalue<Derived>::value),
+                         Scalar *,
+                         const Scalar *>::type
+                     PointerType;
+
+    using Base::derived;
+//    using Base::RowsAtCompileTime;
+//    using Base::ColsAtCompileTime;
+//    using Base::SizeAtCompileTime;
+    using Base::MaxRowsAtCompileTime;
+    using Base::MaxColsAtCompileTime;
+    using Base::MaxSizeAtCompileTime;
+    using Base::IsVectorAtCompileTime;
+    using Base::Flags;
+    using Base::IsRowMajor;
+
+    using Base::rows;
+    using Base::cols;
+    using Base::size;
+    using Base::coeff;
+    using Base::coeffRef;
+    using Base::lazyAssign;
+    using Base::eval;
+
+    using Base::innerStride;
+    using Base::outerStride;
+    using Base::rowStride;
+    using Base::colStride;
+
+    // bug 217 - compile error on ICC 11.1
+    using Base::operator=;
+
+    typedef typename Base::CoeffReturnType CoeffReturnType;
+
+    /** \copydoc DenseBase::rows() */
+    EIGEN_DEVICE_FUNC inline Index rows() const { return m_rows.value(); }
+    /** \copydoc DenseBase::cols() */
+    EIGEN_DEVICE_FUNC inline Index cols() const { return m_cols.value(); }
+
+    /** Returns a pointer to the first coefficient of the matrix or vector.
+      *
+      * \note When addressing this data, make sure to honor the strides returned by innerStride() and outerStride().
+      *
+      * \sa innerStride(), outerStride()
+      */
+    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_data; }
+
+    /** \copydoc PlainObjectBase::coeff(Index,Index) const */
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeff(Index rowId, Index colId) const
+    {
+      return m_data[colId * colStride() + rowId * rowStride()];
+    }
+
+    /** \copydoc PlainObjectBase::coeff(Index) const */
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeff(Index index) const
+    {
+      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+      return m_data[index * innerStride()];
+    }
+
+    /** \copydoc PlainObjectBase::coeffRef(Index,Index) const */
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index rowId, Index colId) const
+    {
+      return this->m_data[colId * colStride() + rowId * rowStride()];
+    }
+
+    /** \copydoc PlainObjectBase::coeffRef(Index) const */
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index index) const
+    {
+      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+      return this->m_data[index * innerStride()];
+    }
+
+    /** \internal */
+    template<int LoadMode>
+    inline PacketScalar packet(Index rowId, Index colId) const
+    {
+      return internal::ploadt<PacketScalar, LoadMode>
+               (m_data + (colId * colStride() + rowId * rowStride()));
+    }
+
+    /** \internal */
+    template<int LoadMode>
+    inline PacketScalar packet(Index index) const
+    {
+      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+      return internal::ploadt<PacketScalar, LoadMode>(m_data + index * innerStride());
+    }
+
+    /** \internal Constructor for fixed size matrices or vectors */
+    EIGEN_DEVICE_FUNC
+    explicit inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime)
+    {
+      EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+      checkSanity<Derived>();
+    }
+
+    /** \internal Constructor for dynamically sized vectors */
+    EIGEN_DEVICE_FUNC
+    inline MapBase(PointerType dataPtr, Index vecSize)
+            : m_data(dataPtr),
+              m_rows(RowsAtCompileTime == Dynamic ? vecSize : Index(RowsAtCompileTime)),
+              m_cols(ColsAtCompileTime == Dynamic ? vecSize : Index(ColsAtCompileTime))
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+      eigen_assert(vecSize >= 0);
+      eigen_assert(dataPtr == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == vecSize);
+      checkSanity<Derived>();
+    }
+
+    /** \internal Constructor for dynamically sized matrices */
+    EIGEN_DEVICE_FUNC
+    inline MapBase(PointerType dataPtr, Index rows, Index cols)
+            : m_data(dataPtr), m_rows(rows), m_cols(cols)
+    {
+      eigen_assert( (dataPtr == 0)
+              || (   rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
+                  && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)));
+      checkSanity<Derived>();
+    }
+
+    #ifdef EIGEN_MAPBASE_PLUGIN
+    #include EIGEN_MAPBASE_PLUGIN
+    #endif
+
+  protected:
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase)
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase)
+
+    template<typename T>
+    EIGEN_DEVICE_FUNC
+    void checkSanity(typename internal::enable_if<(internal::traits<T>::Alignment>0),void*>::type = 0) const
+    {
+#if EIGEN_MAX_ALIGN_BYTES>0
+      // innerStride() is not set yet when this function is called, so we optimistically assume the lowest plausible value:
+      const Index minInnerStride = InnerStrideAtCompileTime == Dynamic ? 1 : Index(InnerStrideAtCompileTime);
+      EIGEN_ONLY_USED_FOR_DEBUG(minInnerStride);
+      eigen_assert((   ((internal::UIntPtr(m_data) % internal::traits<Derived>::Alignment) == 0)
+                    || (cols() * rows() * minInnerStride * sizeof(Scalar)) < internal::traits<Derived>::Alignment ) && "data is not aligned");
+#endif
+    }
+
+    template<typename T>
+    EIGEN_DEVICE_FUNC
+    void checkSanity(typename internal::enable_if<internal::traits<T>::Alignment==0,void*>::type = 0) const
+    {}
+
+    PointerType m_data;
+    const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
+    const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
+};
+
+/** \ingroup Core_Module
+  *
+  * \brief Base class for non-const dense Map and Block expression with direct access
+  *
+  * This base class provides the non-const low-level accessors (e.g. coeff and coeffRef) of
+  * dense Map and Block objects with direct access.
+  * It inherits MapBase<Derived, ReadOnlyAccessors> which defines the const variant for reading specific entries.
+  *
+  * \sa class Map, class Block
+  */
+template<typename Derived> class MapBase<Derived, WriteAccessors>
+  : public MapBase<Derived, ReadOnlyAccessors>
+{
+    typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
+  public:
+
+    typedef MapBase<Derived, ReadOnlyAccessors> Base;
+
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::PacketScalar PacketScalar;
+    typedef typename Base::StorageIndex StorageIndex;
+    typedef typename Base::PointerType PointerType;
+
+    using Base::derived;
+    using Base::rows;
+    using Base::cols;
+    using Base::size;
+    using Base::coeff;
+    using Base::coeffRef;
+
+    using Base::innerStride;
+    using Base::outerStride;
+    using Base::rowStride;
+    using Base::colStride;
+
+    typedef typename internal::conditional<
+                    internal::is_lvalue<Derived>::value,
+                    Scalar,
+                    const Scalar
+                  >::type ScalarWithConstIfNotLvalue;
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar* data() const { return this->m_data; }
+    EIGEN_DEVICE_FUNC
+    inline ScalarWithConstIfNotLvalue* data() { return this->m_data; } // no const-cast here so non-const-correct code will give a compile error
+
+    EIGEN_DEVICE_FUNC
+    inline ScalarWithConstIfNotLvalue& coeffRef(Index row, Index col)
+    {
+      return this->m_data[col * colStride() + row * rowStride()];
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline ScalarWithConstIfNotLvalue& coeffRef(Index index)
+    {
+      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+      return this->m_data[index * innerStride()];
+    }
+
+    template<int StoreMode>
+    inline void writePacket(Index row, Index col, const PacketScalar& val)
+    {
+      internal::pstoret<Scalar, PacketScalar, StoreMode>
+               (this->m_data + (col * colStride() + row * rowStride()), val);
+    }
+
+    template<int StoreMode>
+    inline void writePacket(Index index, const PacketScalar& val)
+    {
+      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
+      internal::pstoret<Scalar, PacketScalar, StoreMode>
+                (this->m_data + index * innerStride(), val);
+    }
+
+    EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {}
+    EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {}
+    EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols) : Base(dataPtr, rows, cols) {}
+
+    EIGEN_DEVICE_FUNC
+    Derived& operator=(const MapBase& other)
+    {
+      ReadOnlyMapBase::Base::operator=(other);
+      return derived();
+    }
+
+    // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base,
+    // see bugs 821 and 920.
+    using ReadOnlyMapBase::Base::operator=;
+  protected:
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(MapBase)
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MapBase)
+};
+
+#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS
+
+} // end namespace Eigen
+
+#endif // EIGEN_MAPBASE_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
new file mode 100644
index 0000000..f8bcc8c
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
@@ -0,0 +1,530 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MATRIXBASE_H
+#define EIGEN_MATRIXBASE_H
+
+namespace Eigen {
+
+/** \class MatrixBase
+  * \ingroup Core_Module
+  *
+  * \brief Base class for all dense matrices, vectors, and expressions
+  *
+  * This class is the base that is inherited by all matrix, vector, and related expression
+  * types. Most of the Eigen API is contained in this class, and its base classes. Other important
+  * classes for the Eigen API are Matrix, and VectorwiseOp.
+  *
+  * Note that some methods are defined in other modules such as the \ref LU_Module LU module
+  * for all functions related to matrix inversions.
+  *
+  * \tparam Derived is the derived type, e.g. a matrix type, or an expression, etc.
+  *
+  * When writing a function taking Eigen objects as argument, if you want your function
+  * to take as argument any matrix, vector, or expression, just let it take a
+  * MatrixBase argument. As an example, here is a function printFirstRow which, given
+  * a matrix, vector, or expression \a x, prints the first row of \a x.
+  *
+  * \code
+    template<typename Derived>
+    void printFirstRow(const Eigen::MatrixBase<Derived>& x)
+    {
+      cout << x.row(0) << endl;
+    }
+  * \endcode
+  *
+  * This class can be extended with the help of the plugin mechanism described on the page
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_MATRIXBASE_PLUGIN.
+  *
+  * \sa \blank \ref TopicClassHierarchy
+  */
+template<typename Derived> class MatrixBase
+  : public DenseBase<Derived>
+{
+  public:
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    typedef MatrixBase StorageBaseType;
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    typedef DenseBase<Derived> Base;
+    using Base::RowsAtCompileTime;
+    using Base::ColsAtCompileTime;
+    using Base::SizeAtCompileTime;
+    using Base::MaxRowsAtCompileTime;
+    using Base::MaxColsAtCompileTime;
+    using Base::MaxSizeAtCompileTime;
+    using Base::IsVectorAtCompileTime;
+    using Base::Flags;
+
+    using Base::derived;
+    using Base::const_cast_derived;
+    using Base::rows;
+    using Base::cols;
+    using Base::size;
+    using Base::coeff;
+    using Base::coeffRef;
+    using Base::lazyAssign;
+    using Base::eval;
+    using Base::operator+=;
+    using Base::operator-=;
+    using Base::operator*=;
+    using Base::operator/=;
+
+    typedef typename Base::CoeffReturnType CoeffReturnType;
+    typedef typename Base::ConstTransposeReturnType ConstTransposeReturnType;
+    typedef typename Base::RowXpr RowXpr;
+    typedef typename Base::ColXpr ColXpr;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** type of the equivalent square matrix */
+    typedef Matrix<Scalar,EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime),
+                          EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    /** \returns the size of the main diagonal, which is min(rows(),cols()).
+      * \sa rows(), cols(), SizeAtCompileTime. */
+    EIGEN_DEVICE_FUNC
+    inline Index diagonalSize() const { return (numext::mini)(rows(),cols()); }
+
+    typedef typename Base::PlainObject PlainObject;
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal Represents a matrix with all coefficients equal to one another*/
+    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
+    /** \internal the return type of MatrixBase::adjoint() */
+    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
+                        CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, ConstTransposeReturnType>,
+                        ConstTransposeReturnType
+                     >::type AdjointReturnType;
+    /** \internal Return type of eigenvalues() */
+    typedef Matrix<std::complex<RealScalar>, internal::traits<Derived>::ColsAtCompileTime, 1, ColMajor> EigenvaluesReturnType;
+    /** \internal the return type of identity */
+    typedef CwiseNullaryOp<internal::scalar_identity_op<Scalar>,PlainObject> IdentityReturnType;
+    /** \internal the return type of unit vectors */
+    typedef Block<const CwiseNullaryOp<internal::scalar_identity_op<Scalar>, SquareMatrixType>,
+                  internal::traits<Derived>::RowsAtCompileTime,
+                  internal::traits<Derived>::ColsAtCompileTime> BasisReturnType;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::MatrixBase
+#define EIGEN_DOC_UNARY_ADDONS(X,Y)
+#   include "../plugins/CommonCwiseUnaryOps.h"
+#   include "../plugins/CommonCwiseBinaryOps.h"
+#   include "../plugins/MatrixCwiseUnaryOps.h"
+#   include "../plugins/MatrixCwiseBinaryOps.h"
+#   ifdef EIGEN_MATRIXBASE_PLUGIN
+#     include EIGEN_MATRIXBASE_PLUGIN
+#   endif
+#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
+#undef EIGEN_DOC_UNARY_ADDONS
+
+    /** Special case of the template operator=, in order to prevent the compiler
+      * from generating a default operator= (issue hit with g++ 4.1)
+      */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const MatrixBase& other);
+
+    // We cannot inherit here via Base::operator= since it is causing
+    // trouble with MSVC.
+
+    template <typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator=(const DenseBase<OtherDerived>& other);
+
+    template <typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    Derived& operator=(const EigenBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    Derived& operator=(const ReturnByValue<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator+=(const MatrixBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Derived& operator-=(const MatrixBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    const Product<Derived,OtherDerived>
+    operator*(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    const Product<Derived,OtherDerived,LazyProduct>
+    lazyProduct(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    Derived& operator*=(const EigenBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    void applyOnTheLeft(const EigenBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    void applyOnTheRight(const EigenBase<OtherDerived>& other);
+
+    template<typename DiagonalDerived>
+    EIGEN_DEVICE_FUNC
+    const Product<Derived, DiagonalDerived, LazyProduct>
+    operator*(const DiagonalBase<DiagonalDerived> &diagonal) const;
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
+    dot(const MatrixBase<OtherDerived>& other) const;
+
+    EIGEN_DEVICE_FUNC RealScalar squaredNorm() const;
+    EIGEN_DEVICE_FUNC RealScalar norm() const;
+    RealScalar stableNorm() const;
+    RealScalar blueNorm() const;
+    RealScalar hypotNorm() const;
+    EIGEN_DEVICE_FUNC const PlainObject normalized() const;
+    EIGEN_DEVICE_FUNC const PlainObject stableNormalized() const;
+    EIGEN_DEVICE_FUNC void normalize();
+    EIGEN_DEVICE_FUNC void stableNormalize();
+
+    EIGEN_DEVICE_FUNC const AdjointReturnType adjoint() const;
+    EIGEN_DEVICE_FUNC void adjointInPlace();
+
+    typedef Diagonal<Derived> DiagonalReturnType;
+    EIGEN_DEVICE_FUNC
+    DiagonalReturnType diagonal();
+
+    typedef typename internal::add_const<Diagonal<const Derived> >::type ConstDiagonalReturnType;
+    EIGEN_DEVICE_FUNC
+    ConstDiagonalReturnType diagonal() const;
+
+    template<int Index> struct DiagonalIndexReturnType { typedef Diagonal<Derived,Index> Type; };
+    template<int Index> struct ConstDiagonalIndexReturnType { typedef const Diagonal<const Derived,Index> Type; };
+
+    template<int Index>
+    EIGEN_DEVICE_FUNC
+    typename DiagonalIndexReturnType<Index>::Type diagonal();
+
+    template<int Index>
+    EIGEN_DEVICE_FUNC
+    typename ConstDiagonalIndexReturnType<Index>::Type diagonal() const;
+
+    typedef Diagonal<Derived,DynamicIndex> DiagonalDynamicIndexReturnType;
+    typedef typename internal::add_const<Diagonal<const Derived,DynamicIndex> >::type ConstDiagonalDynamicIndexReturnType;
+
+    EIGEN_DEVICE_FUNC
+    DiagonalDynamicIndexReturnType diagonal(Index index);
+    EIGEN_DEVICE_FUNC
+    ConstDiagonalDynamicIndexReturnType diagonal(Index index) const;
+
+    template<unsigned int Mode> struct TriangularViewReturnType { typedef TriangularView<Derived, Mode> Type; };
+    template<unsigned int Mode> struct ConstTriangularViewReturnType { typedef const TriangularView<const Derived, Mode> Type; };
+
+    template<unsigned int Mode>
+    EIGEN_DEVICE_FUNC
+    typename TriangularViewReturnType<Mode>::Type triangularView();
+    template<unsigned int Mode>
+    EIGEN_DEVICE_FUNC
+    typename ConstTriangularViewReturnType<Mode>::Type triangularView() const;
+
+    template<unsigned int UpLo> struct SelfAdjointViewReturnType { typedef SelfAdjointView<Derived, UpLo> Type; };
+    template<unsigned int UpLo> struct ConstSelfAdjointViewReturnType { typedef const SelfAdjointView<const Derived, UpLo> Type; };
+
+    template<unsigned int UpLo>
+    EIGEN_DEVICE_FUNC
+    typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
+    template<unsigned int UpLo>
+    EIGEN_DEVICE_FUNC
+    typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
+
+    const SparseView<Derived> sparseView(const Scalar& m_reference = Scalar(0),
+                                         const typename NumTraits<Scalar>::Real& m_epsilon = NumTraits<Scalar>::dummy_precision()) const;
+    EIGEN_DEVICE_FUNC static const IdentityReturnType Identity();
+    EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(Index rows, Index cols);
+    EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index size, Index i);
+    EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index i);
+    EIGEN_DEVICE_FUNC static const BasisReturnType UnitX();
+    EIGEN_DEVICE_FUNC static const BasisReturnType UnitY();
+    EIGEN_DEVICE_FUNC static const BasisReturnType UnitZ();
+    EIGEN_DEVICE_FUNC static const BasisReturnType UnitW();
+
+    EIGEN_DEVICE_FUNC
+    const DiagonalWrapper<const Derived> asDiagonal() const;
+    const PermutationWrapper<const Derived> asPermutation() const;
+
+    EIGEN_DEVICE_FUNC
+    Derived& setIdentity();
+    EIGEN_DEVICE_FUNC
+    Derived& setIdentity(Index rows, Index cols);
+
+    bool isIdentity(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    bool isDiagonal(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
+    bool isUpperTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    bool isLowerTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
+    template<typename OtherDerived>
+    bool isOrthogonal(const MatrixBase<OtherDerived>& other,
+                      const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+    bool isUnitary(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
+
+    /** \returns true if each coefficients of \c *this and \a other are all exactly equal.
+      * \warning When using floating point scalar values you probably should rather use a
+      *          fuzzy comparison such as isApprox()
+      * \sa isApprox(), operator!= */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC inline bool operator==(const MatrixBase<OtherDerived>& other) const
+    { return cwiseEqual(other).all(); }
+
+    /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other.
+      * \warning When using floating point scalar values you probably should rather use a
+      *          fuzzy comparison such as isApprox()
+      * \sa isApprox(), operator== */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC inline bool operator!=(const MatrixBase<OtherDerived>& other) const
+    { return cwiseNotEqual(other).any(); }
+
+    NoAlias<Derived,Eigen::MatrixBase > noalias();
+
+    // TODO forceAlignedAccess is temporarily disabled
+    // Need to find a nicer workaround.
+    inline const Derived& forceAlignedAccess() const { return derived(); }
+    inline Derived& forceAlignedAccess() { return derived(); }
+    template<bool Enable> inline const Derived& forceAlignedAccessIf() const { return derived(); }
+    template<bool Enable> inline Derived& forceAlignedAccessIf() { return derived(); }
+
+    EIGEN_DEVICE_FUNC Scalar trace() const;
+
+    template<int p> EIGEN_DEVICE_FUNC RealScalar lpNorm() const;
+
+    EIGEN_DEVICE_FUNC MatrixBase<Derived>& matrix() { return *this; }
+    EIGEN_DEVICE_FUNC const MatrixBase<Derived>& matrix() const { return *this; }
+
+    /** \returns an \link Eigen::ArrayBase Array \endlink expression of this matrix
+      * \sa ArrayBase::matrix() */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ArrayWrapper<Derived> array() { return ArrayWrapper<Derived>(derived()); }
+    /** \returns a const \link Eigen::ArrayBase Array \endlink expression of this matrix
+      * \sa ArrayBase::matrix() */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ArrayWrapper<const Derived> array() const { return ArrayWrapper<const Derived>(derived()); }
+
+/////////// LU module ///////////
+
+    inline const FullPivLU<PlainObject> fullPivLu() const;
+    inline const PartialPivLU<PlainObject> partialPivLu() const;
+
+    inline const PartialPivLU<PlainObject> lu() const;
+
+    inline const Inverse<Derived> inverse() const;
+
+    template<typename ResultType>
+    inline void computeInverseAndDetWithCheck(
+      ResultType& inverse,
+      typename ResultType::Scalar& determinant,
+      bool& invertible,
+      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
+    ) const;
+    template<typename ResultType>
+    inline void computeInverseWithCheck(
+      ResultType& inverse,
+      bool& invertible,
+      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
+    ) const;
+    Scalar determinant() const;
+
+/////////// Cholesky module ///////////
+
+    inline const LLT<PlainObject>  llt() const;
+    inline const LDLT<PlainObject> ldlt() const;
+
+/////////// QR module ///////////
+
+    inline const HouseholderQR<PlainObject> householderQr() const;
+    inline const ColPivHouseholderQR<PlainObject> colPivHouseholderQr() const;
+    inline const FullPivHouseholderQR<PlainObject> fullPivHouseholderQr() const;
+    inline const CompleteOrthogonalDecomposition<PlainObject> completeOrthogonalDecomposition() const;
+
+/////////// Eigenvalues module ///////////
+
+    inline EigenvaluesReturnType eigenvalues() const;
+    inline RealScalar operatorNorm() const;
+
+/////////// SVD module ///////////
+
+    inline JacobiSVD<PlainObject> jacobiSvd(unsigned int computationOptions = 0) const;
+    inline BDCSVD<PlainObject>    bdcSvd(unsigned int computationOptions = 0) const;
+
+/////////// Geometry module ///////////
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    /// \internal helper struct to form the return type of the cross product
+    template<typename OtherDerived> struct cross_product_return_type {
+      typedef typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType Scalar;
+      typedef Matrix<Scalar,MatrixBase::RowsAtCompileTime,MatrixBase::ColsAtCompileTime> type;
+    };
+    #endif // EIGEN_PARSED_BY_DOXYGEN
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    inline typename cross_product_return_type<OtherDerived>::type
+#else
+    inline PlainObject
+#endif
+    cross(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    inline PlainObject cross3(const MatrixBase<OtherDerived>& other) const;
+
+    EIGEN_DEVICE_FUNC
+    inline PlainObject unitOrthogonal(void) const;
+
+    EIGEN_DEVICE_FUNC
+    inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2) const;
+
+    // put this as separate enum value to work around possible GCC 4.3 bug (?)
+    enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits<Derived>::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical)
+                                          : ColsAtCompileTime==1 ? Vertical : Horizontal };
+    typedef Homogeneous<Derived, HomogeneousReturnTypeDirection> HomogeneousReturnType;
+    EIGEN_DEVICE_FUNC
+    inline HomogeneousReturnType homogeneous() const;
+
+    enum {
+      SizeMinusOne = SizeAtCompileTime==Dynamic ? Dynamic : SizeAtCompileTime-1
+    };
+    typedef Block<const Derived,
+                  internal::traits<Derived>::ColsAtCompileTime==1 ? SizeMinusOne : 1,
+                  internal::traits<Derived>::ColsAtCompileTime==1 ? 1 : SizeMinusOne> ConstStartMinusOne;
+    typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(ConstStartMinusOne,Scalar,quotient) HNormalizedReturnType;
+    EIGEN_DEVICE_FUNC
+    inline const HNormalizedReturnType hnormalized() const;
+
+////////// Householder module ///////////
+
+    void makeHouseholderInPlace(Scalar& tau, RealScalar& beta);
+    template<typename EssentialPart>
+    void makeHouseholder(EssentialPart& essential,
+                         Scalar& tau, RealScalar& beta) const;
+    template<typename EssentialPart>
+    void applyHouseholderOnTheLeft(const EssentialPart& essential,
+                                   const Scalar& tau,
+                                   Scalar* workspace);
+    template<typename EssentialPart>
+    void applyHouseholderOnTheRight(const EssentialPart& essential,
+                                    const Scalar& tau,
+                                    Scalar* workspace);
+
+///////// Jacobi module /////////
+
+    template<typename OtherScalar>
+    void applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j);
+    template<typename OtherScalar>
+    void applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j);
+
+///////// SparseCore module /////////
+
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE const typename SparseMatrixBase<OtherDerived>::template CwiseProductDenseReturnType<Derived>::Type
+    cwiseProduct(const SparseMatrixBase<OtherDerived> &other) const
+    {
+      return other.cwiseProduct(derived());
+    }
+
+///////// MatrixFunctions module /////////
+
+    typedef typename internal::stem_function<Scalar>::type StemFunction;
+#define EIGEN_MATRIX_FUNCTION(ReturnType, Name, Description) \
+    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
+    const ReturnType<Derived> Name() const;
+#define EIGEN_MATRIX_FUNCTION_1(ReturnType, Name, Description, Argument) \
+    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
+    const ReturnType<Derived> Name(Argument) const;
+
+    EIGEN_MATRIX_FUNCTION(MatrixExponentialReturnValue, exp, exponential)
+    /** \brief Helper function for the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>.*/
+    const MatrixFunctionReturnValue<Derived> matrixFunction(StemFunction f) const;
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cosh, hyperbolic cosine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sinh, hyperbolic sine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cos, cosine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sin, sine)
+    EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root)
+    EIGEN_MATRIX_FUNCTION(MatrixLogarithmReturnValue, log, logarithm)
+    EIGEN_MATRIX_FUNCTION_1(MatrixPowerReturnValue,        pow, power to \c p, const RealScalar& p)
+    EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex<RealScalar>& p)
+
+  protected:
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(MatrixBase)
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(MatrixBase)
+
+  private:
+    EIGEN_DEVICE_FUNC explicit MatrixBase(int);
+    EIGEN_DEVICE_FUNC MatrixBase(int,int);
+    template<typename OtherDerived> EIGEN_DEVICE_FUNC explicit MatrixBase(const MatrixBase<OtherDerived>&);
+  protected:
+    // mixing arrays and matrices is not legal
+    template<typename OtherDerived> Derived& operator+=(const ArrayBase<OtherDerived>& )
+    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+    // mixing arrays and matrices is not legal
+    template<typename OtherDerived> Derived& operator-=(const ArrayBase<OtherDerived>& )
+    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
+};
+
+
+/***************************************************************************
+* Implementation of matrix base methods
+***************************************************************************/
+
+/** replaces \c *this by \c *this * \a other.
+  *
+  * \returns a reference to \c *this
+  *
+  * Example: \include MatrixBase_applyOnTheRight.cpp
+  * Output: \verbinclude MatrixBase_applyOnTheRight.out
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline Derived&
+MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived> &other)
+{
+  other.derived().applyThisOnTheRight(derived());
+  return derived();
+}
+
+/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=().
+  *
+  * Example: \include MatrixBase_applyOnTheRight.cpp
+  * Output: \verbinclude MatrixBase_applyOnTheRight.out
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived> &other)
+{
+  other.derived().applyThisOnTheRight(derived());
+}
+
+/** replaces \c *this by \a other * \c *this.
+  *
+  * Example: \include MatrixBase_applyOnTheLeft.cpp
+  * Output: \verbinclude MatrixBase_applyOnTheLeft.out
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived> &other)
+{
+  other.derived().applyThisOnTheLeft(derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_MATRIXBASE_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Product.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Product.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Random.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Random.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Redux.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Redux.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Ref.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Ref.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Select.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Select.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfCwiseBinaryOp.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Solve.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Solve.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Stride.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Stride.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Swap.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Swap.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
new file mode 100644
index 0000000..960dc45
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
@@ -0,0 +1,405 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2009-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_TRANSPOSE_H
+#define EIGEN_TRANSPOSE_H
+
+namespace Eigen { 
+
+namespace internal {
+template<typename MatrixType>
+struct traits<Transpose<MatrixType> > : public traits<MatrixType>
+{
+  typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
+  typedef typename remove_reference<MatrixTypeNested>::type MatrixTypeNestedPlain;
+  enum {
+    RowsAtCompileTime = MatrixType::ColsAtCompileTime,
+    ColsAtCompileTime = MatrixType::RowsAtCompileTime,
+    MaxRowsAtCompileTime = MatrixType::MaxColsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
+    Flags0 = traits<MatrixTypeNestedPlain>::Flags & ~(LvalueBit | NestByRefBit),
+    Flags1 = Flags0 | FlagsLvalueBit,
+    Flags = Flags1 ^ RowMajorBit,
+    InnerStrideAtCompileTime = inner_stride_at_compile_time<MatrixType>::ret,
+    OuterStrideAtCompileTime = outer_stride_at_compile_time<MatrixType>::ret
+  };
+};
+}
+
+template<typename MatrixType, typename StorageKind> class TransposeImpl;
+
+/** \class Transpose
+  * \ingroup Core_Module
+  *
+  * \brief Expression of the transpose of a matrix
+  *
+  * \tparam MatrixType the type of the object of which we are taking the transpose
+  *
+  * This class represents an expression of the transpose of a matrix.
+  * It is the return type of MatrixBase::transpose() and MatrixBase::adjoint()
+  * and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::transpose(), MatrixBase::adjoint()
+  */
+template<typename MatrixType> class Transpose
+  : public TransposeImpl<MatrixType,typename internal::traits<MatrixType>::StorageKind>
+{
+  public:
+
+    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+
+    typedef typename TransposeImpl<MatrixType,typename internal::traits<MatrixType>::StorageKind>::Base Base;
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Transpose)
+    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
+
+    EIGEN_DEVICE_FUNC
+    explicit inline Transpose(MatrixType& matrix) : m_matrix(matrix) {}
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Transpose)
+
+    EIGEN_DEVICE_FUNC inline Index rows() const { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.rows(); }
+
+    /** \returns the nested expression */
+    EIGEN_DEVICE_FUNC
+    const typename internal::remove_all<MatrixTypeNested>::type&
+    nestedExpression() const { return m_matrix; }
+
+    /** \returns the nested expression */
+    EIGEN_DEVICE_FUNC
+    typename internal::remove_reference<MatrixTypeNested>::type&
+    nestedExpression() { return m_matrix; }
+
+    /** \internal */
+    void resize(Index nrows, Index ncols) {
+      m_matrix.resize(ncols,nrows);
+    }
+
+  protected:
+    typename internal::ref_selector<MatrixType>::non_const_type m_matrix;
+};
+
+namespace internal {
+
+template<typename MatrixType, bool HasDirectAccess = has_direct_access<MatrixType>::ret>
+struct TransposeImpl_base
+{
+  typedef typename dense_xpr_base<Transpose<MatrixType> >::type type;
+};
+
+template<typename MatrixType>
+struct TransposeImpl_base<MatrixType, false>
+{
+  typedef typename dense_xpr_base<Transpose<MatrixType> >::type type;
+};
+
+} // end namespace internal
+
+// Generic API dispatcher
+template<typename XprType, typename StorageKind>
+class TransposeImpl
+  : public internal::generic_xpr_base<Transpose<XprType> >::type
+{
+public:
+  typedef typename internal::generic_xpr_base<Transpose<XprType> >::type Base;
+};
+
+template<typename MatrixType> class TransposeImpl<MatrixType,Dense>
+  : public internal::TransposeImpl_base<MatrixType>::type
+{
+  public:
+
+    typedef typename internal::TransposeImpl_base<MatrixType>::type Base;
+    using Base::coeffRef;
+    EIGEN_DENSE_PUBLIC_INTERFACE(Transpose<MatrixType>)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TransposeImpl)
+
+    EIGEN_DEVICE_FUNC inline Index innerStride() const { return derived().nestedExpression().innerStride(); }
+    EIGEN_DEVICE_FUNC inline Index outerStride() const { return derived().nestedExpression().outerStride(); }
+
+    typedef typename internal::conditional<
+                       internal::is_lvalue<MatrixType>::value,
+                       Scalar,
+                       const Scalar
+                     >::type ScalarWithConstIfNotLvalue;
+
+    EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return derived().nestedExpression().data(); }
+    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return derived().nestedExpression().data(); }
+
+    // FIXME: shall we keep the const version of coeffRef?
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index rowId, Index colId) const
+    {
+      return derived().nestedExpression().coeffRef(colId, rowId);
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline const Scalar& coeffRef(Index index) const
+    {
+      return derived().nestedExpression().coeffRef(index);
+    }
+  protected:
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TransposeImpl)
+};
+
+/** \returns an expression of the transpose of *this.
+  *
+  * Example: \include MatrixBase_transpose.cpp
+  * Output: \verbinclude MatrixBase_transpose.out
+  *
+  * \warning If you want to replace a matrix by its own transpose, do \b NOT do this:
+  * \code
+  * m = m.transpose(); // bug!!! caused by aliasing effect
+  * \endcode
+  * Instead, use the transposeInPlace() method:
+  * \code
+  * m.transposeInPlace();
+  * \endcode
+  * which gives Eigen good opportunities for optimization, or alternatively you can also do:
+  * \code
+  * m = m.transpose().eval();
+  * \endcode
+  *
+  * \sa transposeInPlace(), adjoint() */
+template<typename Derived>
+inline Transpose<Derived>
+DenseBase<Derived>::transpose()
+{
+  return TransposeReturnType(derived());
+}
+
+/** This is the const version of transpose().
+  *
+  * Make sure you read the warning for transpose() !
+  *
+  * \sa transposeInPlace(), adjoint() */
+template<typename Derived>
+inline typename DenseBase<Derived>::ConstTransposeReturnType
+DenseBase<Derived>::transpose() const
+{
+  return ConstTransposeReturnType(derived());
+}
+
+/** \returns an expression of the adjoint (i.e. conjugate transpose) of *this.
+  *
+  * Example: \include MatrixBase_adjoint.cpp
+  * Output: \verbinclude MatrixBase_adjoint.out
+  *
+  * \warning If you want to replace a matrix by its own adjoint, do \b NOT do this:
+  * \code
+  * m = m.adjoint(); // bug!!! caused by aliasing effect
+  * \endcode
+  * Instead, use the adjointInPlace() method:
+  * \code
+  * m.adjointInPlace();
+  * \endcode
+  * which gives Eigen good opportunities for optimization, or alternatively you can also do:
+  * \code
+  * m = m.adjoint().eval();
+  * \endcode
+  *
+  * \sa adjointInPlace(), transpose(), conjugate(), class Transpose, class internal::scalar_conjugate_op */
+template<typename Derived>
+inline const typename MatrixBase<Derived>::AdjointReturnType
+MatrixBase<Derived>::adjoint() const
+{
+  return AdjointReturnType(this->transpose());
+}
+
+/***************************************************************************
+* "in place" transpose implementation
+***************************************************************************/
+
+namespace internal {
+
+template<typename MatrixType,
+  bool IsSquare = (MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime) && MatrixType::RowsAtCompileTime!=Dynamic,
+  bool MatchPacketSize =
+        (int(MatrixType::RowsAtCompileTime) == int(internal::packet_traits<typename MatrixType::Scalar>::size))
+    &&  (internal::evaluator<MatrixType>::Flags&PacketAccessBit) >
+struct inplace_transpose_selector;
+
+template<typename MatrixType>
+struct inplace_transpose_selector<MatrixType,true,false> { // square matrix
+  static void run(MatrixType& m) {
+    m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose());
+  }
+};
+
+// TODO: vectorized path is currently limited to LargestPacketSize x LargestPacketSize cases only.
+template<typename MatrixType>
+struct inplace_transpose_selector<MatrixType,true,true> { // PacketSize x PacketSize
+  static void run(MatrixType& m) {
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename internal::packet_traits<typename MatrixType::Scalar>::type Packet;
+    const Index PacketSize = internal::packet_traits<Scalar>::size;
+    const Index Alignment = internal::evaluator<MatrixType>::Alignment;
+    PacketBlock<Packet> A;
+    for (Index i=0; i<PacketSize; ++i)
+      A.packet[i] = m.template packetByOuterInner<Alignment>(i,0);
+    internal::ptranspose(A);
+    for (Index i=0; i<PacketSize; ++i)
+      m.template writePacket<Alignment>(m.rowIndexByOuterInner(i,0), m.colIndexByOuterInner(i,0), A.packet[i]);
+  }
+};
+
+template<typename MatrixType,bool MatchPacketSize>
+struct inplace_transpose_selector<MatrixType,false,MatchPacketSize> { // non square matrix
+  static void run(MatrixType& m) {
+    if (m.rows()==m.cols())
+      m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose());
+    else
+      m = m.transpose().eval();
+  }
+};
+
+} // end namespace internal
+
+/** This is the "in place" version of transpose(): it replaces \c *this by its own transpose.
+  * Thus, doing
+  * \code
+  * m.transposeInPlace();
+  * \endcode
+  * has the same effect on m as doing
+  * \code
+  * m = m.transpose().eval();
+  * \endcode
+  * and is faster and also safer because in the latter line of code, forgetting the eval() results
+  * in a bug caused by \ref TopicAliasing "aliasing".
+  *
+  * Notice however that this method is only useful if you want to replace a matrix by its own transpose.
+  * If you just need the transpose of a matrix, use transpose().
+  *
+  * \note if the matrix is not square, then \c *this must be a resizable matrix. 
+  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
+  *
+  * \sa transpose(), adjoint(), adjointInPlace() */
+template<typename Derived>
+inline void DenseBase<Derived>::transposeInPlace()
+{
+  eigen_assert((rows() == cols() || (RowsAtCompileTime == Dynamic && ColsAtCompileTime == Dynamic))
+               && "transposeInPlace() called on a non-square non-resizable matrix");
+  internal::inplace_transpose_selector<Derived>::run(derived());
+}
+
+/***************************************************************************
+* "in place" adjoint implementation
+***************************************************************************/
+
+/** This is the "in place" version of adjoint(): it replaces \c *this by its own transpose.
+  * Thus, doing
+  * \code
+  * m.adjointInPlace();
+  * \endcode
+  * has the same effect on m as doing
+  * \code
+  * m = m.adjoint().eval();
+  * \endcode
+  * and is faster and also safer because in the latter line of code, forgetting the eval() results
+  * in a bug caused by aliasing.
+  *
+  * Notice however that this method is only useful if you want to replace a matrix by its own adjoint.
+  * If you just need the adjoint of a matrix, use adjoint().
+  *
+  * \note if the matrix is not square, then \c *this must be a resizable matrix.
+  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
+  *
+  * \sa transpose(), adjoint(), transposeInPlace() */
+template<typename Derived>
+inline void MatrixBase<Derived>::adjointInPlace()
+{
+  derived() = adjoint().eval();
+}
+
+#ifndef EIGEN_NO_DEBUG
+
+// The following is to detect aliasing problems in most common cases.
+
+namespace internal {
+
+template<bool DestIsTransposed, typename OtherDerived>
+struct check_transpose_aliasing_compile_time_selector
+{
+  enum { ret = bool(blas_traits<OtherDerived>::IsTransposed) != DestIsTransposed };
+};
+
+template<bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
+struct check_transpose_aliasing_compile_time_selector<DestIsTransposed,CwiseBinaryOp<BinOp,DerivedA,DerivedB> >
+{
+  enum { ret =    bool(blas_traits<DerivedA>::IsTransposed) != DestIsTransposed
+               || bool(blas_traits<DerivedB>::IsTransposed) != DestIsTransposed
+  };
+};
+
+template<typename Scalar, bool DestIsTransposed, typename OtherDerived>
+struct check_transpose_aliasing_run_time_selector
+{
+  static bool run(const Scalar* dest, const OtherDerived& src)
+  {
+    return (bool(blas_traits<OtherDerived>::IsTransposed) != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src));
+  }
+};
+
+template<typename Scalar, bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
+struct check_transpose_aliasing_run_time_selector<Scalar,DestIsTransposed,CwiseBinaryOp<BinOp,DerivedA,DerivedB> >
+{
+  static bool run(const Scalar* dest, const CwiseBinaryOp<BinOp,DerivedA,DerivedB>& src)
+  {
+    return ((blas_traits<DerivedA>::IsTransposed != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src.lhs())))
+        || ((blas_traits<DerivedB>::IsTransposed != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src.rhs())));
+  }
+};
+
+// the following selector, checkTransposeAliasing_impl, based on MightHaveTransposeAliasing,
+// is because when the condition controlling the assert is known at compile time, ICC emits a warning.
+// This is actually a good warning: in expressions that don't have any transposing, the condition is
+// known at compile time to be false, and using that, we can avoid generating the code of the assert again
+// and again for all these expressions that don't need it.
+
+template<typename Derived, typename OtherDerived,
+         bool MightHaveTransposeAliasing
+                 = check_transpose_aliasing_compile_time_selector
+                     <blas_traits<Derived>::IsTransposed,OtherDerived>::ret
+        >
+struct checkTransposeAliasing_impl
+{
+    static void run(const Derived& dst, const OtherDerived& other)
+    {
+        eigen_assert((!check_transpose_aliasing_run_time_selector
+                      <typename Derived::Scalar,blas_traits<Derived>::IsTransposed,OtherDerived>
+                      ::run(extract_data(dst), other))
+          && "aliasing detected during transposition, use transposeInPlace() "
+             "or evaluate the rhs into a temporary using .eval()");
+
+    }
+};
+
+template<typename Derived, typename OtherDerived>
+struct checkTransposeAliasing_impl<Derived, OtherDerived, false>
+{
+    static void run(const Derived&, const OtherDerived&)
+    {
+    }
+};
+
+template<typename Dst, typename Src>
+void check_for_aliasing(const Dst &dst, const Src &src)
+{
+  internal::checkTransposeAliasing_impl<Dst, Src>::run(dst, src);
+}
+
+} // end namespace internal
+
+#endif // EIGEN_NO_DEBUG
+
+} // end namespace Eigen
+
+#endif // EIGEN_TRANSPOSE_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
new file mode 100644
index 0000000..9abb7e3
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
@@ -0,0 +1,985 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_TRIANGULARMATRIX_H
+#define EIGEN_TRIANGULARMATRIX_H
+
+namespace Eigen { 
+
+namespace internal {
+  
+template<int Side, typename TriangularType, typename Rhs> struct triangular_solve_retval;
+  
+}
+
+/** \class TriangularBase
+  * \ingroup Core_Module
+  *
+  * \brief Base class for triangular part in a matrix
+  */
+template<typename Derived> class TriangularBase : public EigenBase<Derived>
+{
+  public:
+
+    enum {
+      Mode = internal::traits<Derived>::Mode,
+      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
+      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
+      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
+      
+      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
+                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
+      /**< This is equal to the number of coefficients, i.e. the number of
+          * rows times the number of columns, or to \a Dynamic if this is not
+          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+      
+      MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
+                                                   internal::traits<Derived>::MaxColsAtCompileTime>::ret)
+        
+    };
+    typedef typename internal::traits<Derived>::Scalar Scalar;
+    typedef typename internal::traits<Derived>::StorageKind StorageKind;
+    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
+    typedef typename internal::traits<Derived>::FullMatrixType DenseMatrixType;
+    typedef DenseMatrixType DenseType;
+    typedef Derived const& Nested;
+
+    EIGEN_DEVICE_FUNC
+    inline TriangularBase() { eigen_assert(!((Mode&UnitDiag) && (Mode&ZeroDiag))); }
+
+    EIGEN_DEVICE_FUNC
+    inline Index rows() const { return derived().rows(); }
+    EIGEN_DEVICE_FUNC
+    inline Index cols() const { return derived().cols(); }
+    EIGEN_DEVICE_FUNC
+    inline Index outerStride() const { return derived().outerStride(); }
+    EIGEN_DEVICE_FUNC
+    inline Index innerStride() const { return derived().innerStride(); }
+    
+    // dummy resize function
+    void resize(Index rows, Index cols)
+    {
+      EIGEN_UNUSED_VARIABLE(rows);
+      EIGEN_UNUSED_VARIABLE(cols);
+      eigen_assert(rows==this->rows() && cols==this->cols());
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline Scalar coeff(Index row, Index col) const  { return derived().coeff(row,col); }
+    EIGEN_DEVICE_FUNC
+    inline Scalar& coeffRef(Index row, Index col) { return derived().coeffRef(row,col); }
+
+    /** \see MatrixBase::copyCoeff(row,col)
+      */
+    template<typename Other>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE void copyCoeff(Index row, Index col, Other& other)
+    {
+      derived().coeffRef(row, col) = other.coeff(row, col);
+    }
+
+    EIGEN_DEVICE_FUNC
+    inline Scalar operator()(Index row, Index col) const
+    {
+      check_coordinates(row, col);
+      return coeff(row,col);
+    }
+    EIGEN_DEVICE_FUNC
+    inline Scalar& operator()(Index row, Index col)
+    {
+      check_coordinates(row, col);
+      return coeffRef(row,col);
+    }
+
+    #ifndef EIGEN_PARSED_BY_DOXYGEN
+    EIGEN_DEVICE_FUNC
+    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    EIGEN_DEVICE_FUNC
+    inline Derived& derived() { return *static_cast<Derived*>(this); }
+    #endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    template<typename DenseDerived>
+    EIGEN_DEVICE_FUNC
+    void evalTo(MatrixBase<DenseDerived> &other) const;
+    template<typename DenseDerived>
+    EIGEN_DEVICE_FUNC
+    void evalToLazy(MatrixBase<DenseDerived> &other) const;
+
+    EIGEN_DEVICE_FUNC
+    DenseMatrixType toDenseMatrix() const
+    {
+      DenseMatrixType res(rows(), cols());
+      evalToLazy(res);
+      return res;
+    }
+
+  protected:
+
+    void check_coordinates(Index row, Index col) const
+    {
+      EIGEN_ONLY_USED_FOR_DEBUG(row);
+      EIGEN_ONLY_USED_FOR_DEBUG(col);
+      eigen_assert(col>=0 && col<cols() && row>=0 && row<rows());
+      const int mode = int(Mode) & ~SelfAdjoint;
+      EIGEN_ONLY_USED_FOR_DEBUG(mode);
+      eigen_assert((mode==Upper && col>=row)
+                || (mode==Lower && col<=row)
+                || ((mode==StrictlyUpper || mode==UnitUpper) && col>row)
+                || ((mode==StrictlyLower || mode==UnitLower) && col<row));
+    }
+
+    #ifdef EIGEN_INTERNAL_DEBUGGING
+    void check_coordinates_internal(Index row, Index col) const
+    {
+      check_coordinates(row, col);
+    }
+    #else
+    void check_coordinates_internal(Index , Index ) const {}
+    #endif
+
+};
+
+/** \class TriangularView
+  * \ingroup Core_Module
+  *
+  * \brief Expression of a triangular part in a matrix
+  *
+  * \param MatrixType the type of the object in which we are taking the triangular part
+  * \param Mode the kind of triangular matrix expression to construct. Can be #Upper,
+  *             #Lower, #UnitUpper, #UnitLower, #StrictlyUpper, or #StrictlyLower.
+  *             This is in fact a bit field; it must have either #Upper or #Lower, 
+  *             and additionally it may have #UnitDiag or #ZeroDiag or neither.
+  *
+  * This class represents a triangular part of a matrix, not necessarily square. Strictly speaking, for rectangular
+  * matrices one should speak of "trapezoid" parts. This class is the return type
+  * of MatrixBase::triangularView() and SparseMatrixBase::triangularView(), and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::triangularView()
+  */
+namespace internal {
+template<typename MatrixType, unsigned int _Mode>
+struct traits<TriangularView<MatrixType, _Mode> > : traits<MatrixType>
+{
+  typedef typename ref_selector<MatrixType>::non_const_type MatrixTypeNested;
+  typedef typename remove_reference<MatrixTypeNested>::type MatrixTypeNestedNonRef;
+  typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
+  typedef typename MatrixType::PlainObject FullMatrixType;
+  typedef MatrixType ExpressionType;
+  enum {
+    Mode = _Mode,
+    FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
+    Flags = (MatrixTypeNestedCleaned::Flags & (HereditaryBits | FlagsLvalueBit) & (~(PacketAccessBit | DirectAccessBit | LinearAccessBit)))
+  };
+};
+}
+
+template<typename _MatrixType, unsigned int _Mode, typename StorageKind> class TriangularViewImpl;
+
+template<typename _MatrixType, unsigned int _Mode> class TriangularView
+  : public TriangularViewImpl<_MatrixType, _Mode, typename internal::traits<_MatrixType>::StorageKind >
+{
+  public:
+
+    typedef TriangularViewImpl<_MatrixType, _Mode, typename internal::traits<_MatrixType>::StorageKind > Base;
+    typedef typename internal::traits<TriangularView>::Scalar Scalar;
+    typedef _MatrixType MatrixType;
+
+  protected:
+    typedef typename internal::traits<TriangularView>::MatrixTypeNested MatrixTypeNested;
+    typedef typename internal::traits<TriangularView>::MatrixTypeNestedNonRef MatrixTypeNestedNonRef;
+
+    typedef typename internal::remove_all<typename MatrixType::ConjugateReturnType>::type MatrixConjugateReturnType;
+    
+  public:
+
+    typedef typename internal::traits<TriangularView>::StorageKind StorageKind;
+    typedef typename internal::traits<TriangularView>::MatrixTypeNestedCleaned NestedExpression;
+
+    enum {
+      Mode = _Mode,
+      Flags = internal::traits<TriangularView>::Flags,
+      TransposeMode = (Mode & Upper ? Lower : 0)
+                    | (Mode & Lower ? Upper : 0)
+                    | (Mode & (UnitDiag))
+                    | (Mode & (ZeroDiag)),
+      IsVectorAtCompileTime = false
+    };
+
+    EIGEN_DEVICE_FUNC
+    explicit inline TriangularView(MatrixType& matrix) : m_matrix(matrix)
+    {}
+    
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TriangularView)
+
+    /** \copydoc EigenBase::rows() */
+    EIGEN_DEVICE_FUNC
+    inline Index rows() const { return m_matrix.rows(); }
+    /** \copydoc EigenBase::cols() */
+    EIGEN_DEVICE_FUNC
+    inline Index cols() const { return m_matrix.cols(); }
+
+    /** \returns a const reference to the nested expression */
+    EIGEN_DEVICE_FUNC
+    const NestedExpression& nestedExpression() const { return m_matrix; }
+
+    /** \returns a reference to the nested expression */
+    EIGEN_DEVICE_FUNC
+    NestedExpression& nestedExpression() { return m_matrix; }
+    
+    typedef TriangularView<const MatrixConjugateReturnType,Mode> ConjugateReturnType;
+    /** \sa MatrixBase::conjugate() const */
+    EIGEN_DEVICE_FUNC
+    inline const ConjugateReturnType conjugate() const
+    { return ConjugateReturnType(m_matrix.conjugate()); }
+
+    typedef TriangularView<const typename MatrixType::AdjointReturnType,TransposeMode> AdjointReturnType;
+    /** \sa MatrixBase::adjoint() const */
+    EIGEN_DEVICE_FUNC
+    inline const AdjointReturnType adjoint() const
+    { return AdjointReturnType(m_matrix.adjoint()); }
+
+    typedef TriangularView<typename MatrixType::TransposeReturnType,TransposeMode> TransposeReturnType;
+     /** \sa MatrixBase::transpose() */
+    EIGEN_DEVICE_FUNC
+    inline TransposeReturnType transpose()
+    {
+      EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
+      typename MatrixType::TransposeReturnType tmp(m_matrix);
+      return TransposeReturnType(tmp);
+    }
+    
+    typedef TriangularView<const typename MatrixType::ConstTransposeReturnType,TransposeMode> ConstTransposeReturnType;
+    /** \sa MatrixBase::transpose() const */
+    EIGEN_DEVICE_FUNC
+    inline const ConstTransposeReturnType transpose() const
+    {
+      return ConstTransposeReturnType(m_matrix.transpose());
+    }
+
+    template<typename Other>
+    EIGEN_DEVICE_FUNC
+    inline const Solve<TriangularView, Other> 
+    solve(const MatrixBase<Other>& other) const
+    { return Solve<TriangularView, Other>(*this, other.derived()); }
+    
+  // workaround MSVC ICE
+  #if EIGEN_COMP_MSVC
+    template<int Side, typename Other>
+    EIGEN_DEVICE_FUNC
+    inline const internal::triangular_solve_retval<Side,TriangularView, Other>
+    solve(const MatrixBase<Other>& other) const
+    { return Base::template solve<Side>(other); }
+  #else
+    using Base::solve;
+  #endif
+
+    /** \returns a selfadjoint view of the referenced triangular part which must be either \c #Upper or \c #Lower.
+      *
+      * This is a shortcut for \code this->nestedExpression().selfadjointView<(*this)::Mode>() \endcode
+      * \sa MatrixBase::selfadjointView() */
+    EIGEN_DEVICE_FUNC
+    SelfAdjointView<MatrixTypeNestedNonRef,Mode> selfadjointView()
+    {
+      EIGEN_STATIC_ASSERT((Mode&(UnitDiag|ZeroDiag))==0,PROGRAMMING_ERROR);
+      return SelfAdjointView<MatrixTypeNestedNonRef,Mode>(m_matrix);
+    }
+
+    /** This is the const version of selfadjointView() */
+    EIGEN_DEVICE_FUNC
+    const SelfAdjointView<MatrixTypeNestedNonRef,Mode> selfadjointView() const
+    {
+      EIGEN_STATIC_ASSERT((Mode&(UnitDiag|ZeroDiag))==0,PROGRAMMING_ERROR);
+      return SelfAdjointView<MatrixTypeNestedNonRef,Mode>(m_matrix);
+    }
+
+
+    /** \returns the determinant of the triangular matrix
+      * \sa MatrixBase::determinant() */
+    EIGEN_DEVICE_FUNC
+    Scalar determinant() const
+    {
+      if (Mode & UnitDiag)
+        return 1;
+      else if (Mode & ZeroDiag)
+        return 0;
+      else
+        return m_matrix.diagonal().prod();
+    }
+      
+  protected:
+
+    MatrixTypeNested m_matrix;
+};
+
+/** \ingroup Core_Module
+  *
+  * \brief Base class for a triangular part in a \b dense matrix
+  *
+  * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be instantiated.
+  * It extends class TriangularView with additional methods which available for dense expressions only.
+  *
+  * \sa class TriangularView, MatrixBase::triangularView()
+  */
+template<typename _MatrixType, unsigned int _Mode> class TriangularViewImpl<_MatrixType,_Mode,Dense>
+  : public TriangularBase<TriangularView<_MatrixType, _Mode> >
+{
+  public:
+
+    typedef TriangularView<_MatrixType, _Mode> TriangularViewType;
+    typedef TriangularBase<TriangularViewType> Base;
+    typedef typename internal::traits<TriangularViewType>::Scalar Scalar;
+
+    typedef _MatrixType MatrixType;
+    typedef typename MatrixType::PlainObject DenseMatrixType;
+    typedef DenseMatrixType PlainObject;
+
+  public:
+    using Base::evalToLazy;
+    using Base::derived;
+
+    typedef typename internal::traits<TriangularViewType>::StorageKind StorageKind;
+
+    enum {
+      Mode = _Mode,
+      Flags = internal::traits<TriangularViewType>::Flags
+    };
+
+    /** \returns the outer-stride of the underlying dense matrix
+      * \sa DenseCoeffsBase::outerStride() */
+    EIGEN_DEVICE_FUNC
+    inline Index outerStride() const { return derived().nestedExpression().outerStride(); }
+    /** \returns the inner-stride of the underlying dense matrix
+      * \sa DenseCoeffsBase::innerStride() */
+    EIGEN_DEVICE_FUNC
+    inline Index innerStride() const { return derived().nestedExpression().innerStride(); }
+
+    /** \sa MatrixBase::operator+=() */
+    template<typename Other>
+    EIGEN_DEVICE_FUNC
+    TriangularViewType&  operator+=(const DenseBase<Other>& other) {
+      internal::call_assignment_no_alias(derived(), other.derived(), internal::add_assign_op<Scalar,typename Other::Scalar>());
+      return derived();
+    }
+    /** \sa MatrixBase::operator-=() */
+    template<typename Other>
+    EIGEN_DEVICE_FUNC
+    TriangularViewType&  operator-=(const DenseBase<Other>& other) {
+      internal::call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op<Scalar,typename Other::Scalar>());
+      return derived();
+    }
+    
+    /** \sa MatrixBase::operator*=() */
+    EIGEN_DEVICE_FUNC
+    TriangularViewType&  operator*=(const typename internal::traits<MatrixType>::Scalar& other) { return *this = derived().nestedExpression() * other; }
+    /** \sa DenseBase::operator/=() */
+    EIGEN_DEVICE_FUNC
+    TriangularViewType&  operator/=(const typename internal::traits<MatrixType>::Scalar& other) { return *this = derived().nestedExpression() / other; }
+
+    /** \sa MatrixBase::fill() */
+    EIGEN_DEVICE_FUNC
+    void fill(const Scalar& value) { setConstant(value); }
+    /** \sa MatrixBase::setConstant() */
+    EIGEN_DEVICE_FUNC
+    TriangularViewType& setConstant(const Scalar& value)
+    { return *this = MatrixType::Constant(derived().rows(), derived().cols(), value); }
+    /** \sa MatrixBase::setZero() */
+    EIGEN_DEVICE_FUNC
+    TriangularViewType& setZero() { return setConstant(Scalar(0)); }
+    /** \sa MatrixBase::setOnes() */
+    EIGEN_DEVICE_FUNC
+    TriangularViewType& setOnes() { return setConstant(Scalar(1)); }
+
+    /** \sa MatrixBase::coeff()
+      * \warning the coordinates must fit into the referenced triangular part
+      */
+    EIGEN_DEVICE_FUNC
+    inline Scalar coeff(Index row, Index col) const
+    {
+      Base::check_coordinates_internal(row, col);
+      return derived().nestedExpression().coeff(row, col);
+    }
+
+    /** \sa MatrixBase::coeffRef()
+      * \warning the coordinates must fit into the referenced triangular part
+      */
+    EIGEN_DEVICE_FUNC
+    inline Scalar& coeffRef(Index row, Index col)
+    {
+      EIGEN_STATIC_ASSERT_LVALUE(TriangularViewType);
+      Base::check_coordinates_internal(row, col);
+      return derived().nestedExpression().coeffRef(row, col);
+    }
+
+    /** Assigns a triangular matrix to a triangular part of a dense matrix */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    TriangularViewType& operator=(const TriangularBase<OtherDerived>& other);
+
+    /** Shortcut for\code *this = other.other.triangularView<(*this)::Mode>() \endcode */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    TriangularViewType& operator=(const MatrixBase<OtherDerived>& other);
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    EIGEN_DEVICE_FUNC
+    TriangularViewType& operator=(const TriangularViewImpl& other)
+    { return *this = other.derived().nestedExpression(); }
+
+    /** \deprecated */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    void lazyAssign(const TriangularBase<OtherDerived>& other);
+
+    /** \deprecated */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    void lazyAssign(const MatrixBase<OtherDerived>& other);
+#endif
+
+    /** Efficient triangular matrix times vector/matrix product */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    const Product<TriangularViewType,OtherDerived>
+    operator*(const MatrixBase<OtherDerived>& rhs) const
+    {
+      return Product<TriangularViewType,OtherDerived>(derived(), rhs.derived());
+    }
+
+    /** Efficient vector/matrix times triangular matrix product */
+    template<typename OtherDerived> friend
+    EIGEN_DEVICE_FUNC
+    const Product<OtherDerived,TriangularViewType>
+    operator*(const MatrixBase<OtherDerived>& lhs, const TriangularViewImpl& rhs)
+    {
+      return Product<OtherDerived,TriangularViewType>(lhs.derived(),rhs.derived());
+    }
+
+    /** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
+      *
+      * This function computes the inverse-matrix matrix product inverse(\c *this) * \a other if
+      * \a Side==OnTheLeft (the default), or the right-inverse-multiply  \a other * inverse(\c *this) if
+      * \a Side==OnTheRight.
+      *
+      * Note that the template parameter \c Side can be ommitted, in which case \c Side==OnTheLeft
+      *
+      * The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
+      * diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
+      * is an upper (resp. lower) triangular matrix.
+      *
+      * Example: \include Triangular_solve.cpp
+      * Output: \verbinclude Triangular_solve.out
+      *
+      * This function returns an expression of the inverse-multiply and can works in-place if it is assigned
+      * to the same matrix or vector \a other.
+      *
+      * For users coming from BLAS, this function (and more specifically solveInPlace()) offer
+      * all the operations supported by the \c *TRSV and \c *TRSM BLAS routines.
+      *
+      * \sa TriangularView::solveInPlace()
+      */
+    template<int Side, typename Other>
+    EIGEN_DEVICE_FUNC
+    inline const internal::triangular_solve_retval<Side,TriangularViewType, Other>
+    solve(const MatrixBase<Other>& other) const;
+
+    /** "in-place" version of TriangularView::solve() where the result is written in \a other
+      *
+      * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
+      * This function will const_cast it, so constness isn't honored here.
+      *
+      * Note that the template parameter \c Side can be ommitted, in which case \c Side==OnTheLeft
+      *
+      * See TriangularView:solve() for the details.
+      */
+    template<int Side, typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    void solveInPlace(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    void solveInPlace(const MatrixBase<OtherDerived>& other) const
+    { return solveInPlace<OnTheLeft>(other); }
+
+    /** Swaps the coefficients of the common triangular parts of two matrices */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+    void swap(TriangularBase<OtherDerived> &other)
+#else
+    void swap(TriangularBase<OtherDerived> const & other)
+#endif
+    {
+      EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
+      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
+    }
+
+    /** \deprecated
+      * Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    void swap(MatrixBase<OtherDerived> const & other)
+    {
+      EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
+      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
+    }
+
+    template<typename RhsType, typename DstType>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE void _solve_impl(const RhsType &rhs, DstType &dst) const {
+      if(!internal::is_same_dense(dst,rhs))
+        dst = rhs;
+      this->solveInPlace(dst);
+    }
+
+    template<typename ProductType>
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE TriangularViewType& _assignProduct(const ProductType& prod, const Scalar& alpha, bool beta);
+  protected:
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(TriangularViewImpl)
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(TriangularViewImpl)
+
+};
+
+/***************************************************************************
+* Implementation of triangular evaluation/assignment
+***************************************************************************/
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+// FIXME should we keep that possibility
+template<typename MatrixType, unsigned int Mode>
+template<typename OtherDerived>
+inline TriangularView<MatrixType, Mode>&
+TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const MatrixBase<OtherDerived>& other)
+{
+  internal::call_assignment_no_alias(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
+  return derived();
+}
+
+// FIXME should we keep that possibility
+template<typename MatrixType, unsigned int Mode>
+template<typename OtherDerived>
+void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const MatrixBase<OtherDerived>& other)
+{
+  internal::call_assignment_no_alias(derived(), other.template triangularView<Mode>());
+}
+
+
+
+template<typename MatrixType, unsigned int Mode>
+template<typename OtherDerived>
+inline TriangularView<MatrixType, Mode>&
+TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const TriangularBase<OtherDerived>& other)
+{
+  eigen_assert(Mode == int(OtherDerived::Mode));
+  internal::call_assignment(derived(), other.derived());
+  return derived();
+}
+
+template<typename MatrixType, unsigned int Mode>
+template<typename OtherDerived>
+void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const TriangularBase<OtherDerived>& other)
+{
+  eigen_assert(Mode == int(OtherDerived::Mode));
+  internal::call_assignment_no_alias(derived(), other.derived());
+}
+#endif
+
+/***************************************************************************
+* Implementation of TriangularBase methods
+***************************************************************************/
+
+/** Assigns a triangular or selfadjoint matrix to a dense matrix.
+  * If the matrix is triangular, the opposite part is set to zero. */
+template<typename Derived>
+template<typename DenseDerived>
+void TriangularBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
+{
+  evalToLazy(other.derived());
+}
+
+/***************************************************************************
+* Implementation of TriangularView methods
+***************************************************************************/
+
+/***************************************************************************
+* Implementation of MatrixBase methods
+***************************************************************************/
+
+/**
+  * \returns an expression of a triangular view extracted from the current matrix
+  *
+  * The parameter \a Mode can have the following values: \c #Upper, \c #StrictlyUpper, \c #UnitUpper,
+  * \c #Lower, \c #StrictlyLower, \c #UnitLower.
+  *
+  * Example: \include MatrixBase_triangularView.cpp
+  * Output: \verbinclude MatrixBase_triangularView.out
+  *
+  * \sa class TriangularView
+  */
+template<typename Derived>
+template<unsigned int Mode>
+typename MatrixBase<Derived>::template TriangularViewReturnType<Mode>::Type
+MatrixBase<Derived>::triangularView()
+{
+  return typename TriangularViewReturnType<Mode>::Type(derived());
+}
+
+/** This is the const version of MatrixBase::triangularView() */
+template<typename Derived>
+template<unsigned int Mode>
+typename MatrixBase<Derived>::template ConstTriangularViewReturnType<Mode>::Type
+MatrixBase<Derived>::triangularView() const
+{
+  return typename ConstTriangularViewReturnType<Mode>::Type(derived());
+}
+
+/** \returns true if *this is approximately equal to an upper triangular matrix,
+  *          within the precision given by \a prec.
+  *
+  * \sa isLowerTriangular()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isUpperTriangular(const RealScalar& prec) const
+{
+  RealScalar maxAbsOnUpperPart = static_cast<RealScalar>(-1);
+  for(Index j = 0; j < cols(); ++j)
+  {
+    Index maxi = numext::mini(j, rows()-1);
+    for(Index i = 0; i <= maxi; ++i)
+    {
+      RealScalar absValue = numext::abs(coeff(i,j));
+      if(absValue > maxAbsOnUpperPart) maxAbsOnUpperPart = absValue;
+    }
+  }
+  RealScalar threshold = maxAbsOnUpperPart * prec;
+  for(Index j = 0; j < cols(); ++j)
+    for(Index i = j+1; i < rows(); ++i)
+      if(numext::abs(coeff(i, j)) > threshold) return false;
+  return true;
+}
+
+/** \returns true if *this is approximately equal to a lower triangular matrix,
+  *          within the precision given by \a prec.
+  *
+  * \sa isUpperTriangular()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isLowerTriangular(const RealScalar& prec) const
+{
+  RealScalar maxAbsOnLowerPart = static_cast<RealScalar>(-1);
+  for(Index j = 0; j < cols(); ++j)
+    for(Index i = j; i < rows(); ++i)
+    {
+      RealScalar absValue = numext::abs(coeff(i,j));
+      if(absValue > maxAbsOnLowerPart) maxAbsOnLowerPart = absValue;
+    }
+  RealScalar threshold = maxAbsOnLowerPart * prec;
+  for(Index j = 1; j < cols(); ++j)
+  {
+    Index maxi = numext::mini(j, rows()-1);
+    for(Index i = 0; i < maxi; ++i)
+      if(numext::abs(coeff(i, j)) > threshold) return false;
+  }
+  return true;
+}
+
+
+/***************************************************************************
+****************************************************************************
+* Evaluators and Assignment of triangular expressions
+***************************************************************************
+***************************************************************************/
+
+namespace internal {
+
+  
+// TODO currently a triangular expression has the form TriangularView<.,.>
+//      in the future triangular-ness should be defined by the expression traits
+//      such that Transpose<TriangularView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
+template<typename MatrixType, unsigned int Mode>
+struct evaluator_traits<TriangularView<MatrixType,Mode> >
+{
+  typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
+  typedef typename glue_shapes<typename evaluator_traits<MatrixType>::Shape, TriangularShape>::type Shape;
+};
+
+template<typename MatrixType, unsigned int Mode>
+struct unary_evaluator<TriangularView<MatrixType,Mode>, IndexBased>
+ : evaluator<typename internal::remove_all<MatrixType>::type>
+{
+  typedef TriangularView<MatrixType,Mode> XprType;
+  typedef evaluator<typename internal::remove_all<MatrixType>::type> Base;
+  unary_evaluator(const XprType &xpr) : Base(xpr.nestedExpression()) {}
+};
+
+// Additional assignment kinds:
+struct Triangular2Triangular    {};
+struct Triangular2Dense         {};
+struct Dense2Triangular         {};
+
+
+template<typename Kernel, unsigned int Mode, int UnrollCount, bool ClearOpposite> struct triangular_assignment_loop;
+
+ 
+/** \internal Specialization of the dense assignment kernel for triangular matrices.
+  * The main difference is that the triangular, diagonal, and opposite parts are processed through three different functions.
+  * \tparam UpLo must be either Lower or Upper
+  * \tparam Mode must be either 0, UnitDiag, ZeroDiag, or SelfAdjoint
+  */
+template<int UpLo, int Mode, int SetOpposite, typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor, int Version = Specialized>
+class triangular_dense_assignment_kernel : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version>
+{
+protected:
+  typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version> Base;
+  typedef typename Base::DstXprType DstXprType;
+  typedef typename Base::SrcXprType SrcXprType;
+  using Base::m_dst;
+  using Base::m_src;
+  using Base::m_functor;
+public:
+  
+  typedef typename Base::DstEvaluatorType DstEvaluatorType;
+  typedef typename Base::SrcEvaluatorType SrcEvaluatorType;
+  typedef typename Base::Scalar Scalar;
+  typedef typename Base::AssignmentTraits AssignmentTraits;
+  
+  
+  EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
+    : Base(dst, src, func, dstExpr)
+  {}
+  
+#ifdef EIGEN_INTERNAL_DEBUGGING
+  EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col)
+  {
+    eigen_internal_assert(row!=col);
+    Base::assignCoeff(row,col);
+  }
+#else
+  using Base::assignCoeff;
+#endif
+  
+  EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id)
+  {
+         if(Mode==UnitDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(1));
+    else if(Mode==ZeroDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(0));
+    else if(Mode==0)                       Base::assignCoeff(id,id);
+  }
+  
+  EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index row, Index col)
+  { 
+    eigen_internal_assert(row!=col);
+    if(SetOpposite)
+      m_functor.assignCoeff(m_dst.coeffRef(row,col), Scalar(0));
+  }
+};
+
+template<int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType, typename Functor>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src, const Functor &func)
+{
+  typedef evaluator<DstXprType> DstEvaluatorType;
+  typedef evaluator<SrcXprType> SrcEvaluatorType;
+
+  SrcEvaluatorType srcEvaluator(src);
+
+  Index dstRows = src.rows();
+  Index dstCols = src.cols();
+  if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+    dst.resize(dstRows, dstCols);
+  DstEvaluatorType dstEvaluator(dst);
+    
+  typedef triangular_dense_assignment_kernel< Mode&(Lower|Upper),Mode&(UnitDiag|ZeroDiag|SelfAdjoint),SetOpposite,
+                                              DstEvaluatorType,SrcEvaluatorType,Functor> Kernel;
+  Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
+  
+  enum {
+      unroll = DstXprType::SizeAtCompileTime != Dynamic
+            && SrcEvaluatorType::CoeffReadCost < HugeCost
+            && DstXprType::SizeAtCompileTime * (DstEvaluatorType::CoeffReadCost+SrcEvaluatorType::CoeffReadCost) / 2 <= EIGEN_UNROLLING_LIMIT
+    };
+  
+  triangular_assignment_loop<Kernel, Mode, unroll ? int(DstXprType::SizeAtCompileTime) : Dynamic, SetOpposite>::run(kernel);
+}
+
+template<int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src)
+{
+  call_triangular_assignment_loop<Mode,SetOpposite>(dst, src, internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>());
+}
+
+template<> struct AssignmentKind<TriangularShape,TriangularShape> { typedef Triangular2Triangular Kind; };
+template<> struct AssignmentKind<DenseShape,TriangularShape>      { typedef Triangular2Dense      Kind; };
+template<> struct AssignmentKind<TriangularShape,DenseShape>      { typedef Dense2Triangular      Kind; };
+
+
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Triangular>
+{
+  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
+  {
+    eigen_assert(int(DstXprType::Mode) == int(SrcXprType::Mode));
+    
+    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);  
+  }
+};
+
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Dense>
+{
+  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
+  {
+    call_triangular_assignment_loop<SrcXprType::Mode, (SrcXprType::Mode&SelfAdjoint)==0>(dst, src, func);  
+  }
+};
+
+template< typename DstXprType, typename SrcXprType, typename Functor>
+struct Assignment<DstXprType, SrcXprType, Functor, Dense2Triangular>
+{
+  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
+  {
+    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);  
+  }
+};
+
+
+template<typename Kernel, unsigned int Mode, int UnrollCount, bool SetOpposite>
+struct triangular_assignment_loop
+{
+  // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
+  typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
+  typedef typename DstEvaluatorType::XprType DstXprType;
+  
+  enum {
+    col = (UnrollCount-1) / DstXprType::RowsAtCompileTime,
+    row = (UnrollCount-1) % DstXprType::RowsAtCompileTime
+  };
+  
+  typedef typename Kernel::Scalar Scalar;
+
+  EIGEN_DEVICE_FUNC
+  static inline void run(Kernel &kernel)
+  {
+    triangular_assignment_loop<Kernel, Mode, UnrollCount-1, SetOpposite>::run(kernel);
+    
+    if(row==col)
+      kernel.assignDiagonalCoeff(row);
+    else if( ((Mode&Lower) && row>col) || ((Mode&Upper) && row<col) )
+      kernel.assignCoeff(row,col);
+    else if(SetOpposite)
+      kernel.assignOppositeCoeff(row,col);
+  }
+};
+
+// prevent buggy user code from causing an infinite recursion
+template<typename Kernel, unsigned int Mode, bool SetOpposite>
+struct triangular_assignment_loop<Kernel, Mode, 0, SetOpposite>
+{
+  EIGEN_DEVICE_FUNC
+  static inline void run(Kernel &) {}
+};
+
+
+
+// TODO: experiment with a recursive assignment procedure splitting the current
+//       triangular part into one rectangular and two triangular parts.
+
+
+template<typename Kernel, unsigned int Mode, bool SetOpposite>
+struct triangular_assignment_loop<Kernel, Mode, Dynamic, SetOpposite>
+{
+  typedef typename Kernel::Scalar Scalar;
+  EIGEN_DEVICE_FUNC
+  static inline void run(Kernel &kernel)
+  {
+    for(Index j = 0; j < kernel.cols(); ++j)
+    {
+      Index maxi = numext::mini(j, kernel.rows());
+      Index i = 0;
+      if (((Mode&Lower) && SetOpposite) || (Mode&Upper))
+      {
+        for(; i < maxi; ++i)
+          if(Mode&Upper) kernel.assignCoeff(i, j);
+          else           kernel.assignOppositeCoeff(i, j);
+      }
+      else
+        i = maxi;
+      
+      if(i<kernel.rows()) // then i==j
+        kernel.assignDiagonalCoeff(i++);
+      
+      if (((Mode&Upper) && SetOpposite) || (Mode&Lower))
+      {
+        for(; i < kernel.rows(); ++i)
+          if(Mode&Lower) kernel.assignCoeff(i, j);
+          else           kernel.assignOppositeCoeff(i, j);
+      }
+    }
+  }
+};
+
+} // end namespace internal
+
+/** Assigns a triangular or selfadjoint matrix to a dense matrix.
+  * If the matrix is triangular, the opposite part is set to zero. */
+template<typename Derived>
+template<typename DenseDerived>
+void TriangularBase<Derived>::evalToLazy(MatrixBase<DenseDerived> &other) const
+{
+  other.derived().resize(this->rows(), this->cols());
+  internal::call_triangular_assignment_loop<Derived::Mode,(Derived::Mode&SelfAdjoint)==0 /* SetOpposite */>(other.derived(), derived().nestedExpression());
+}
+
+namespace internal {
+  
+// Triangular = Product
+template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
+{
+  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename SrcXprType::Scalar> &)
+  {
+    Index dstRows = src.rows();
+    Index dstCols = src.cols();
+    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
+      dst.resize(dstRows, dstCols);
+
+    dst._assignProduct(src, 1, 0);
+  }
+};
+
+// Triangular += Product
+template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::add_assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
+{
+  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<Scalar,typename SrcXprType::Scalar> &)
+  {
+    dst._assignProduct(src, 1, 1);
+  }
+};
+
+// Triangular -= Product
+template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
+struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::sub_assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
+{
+  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
+  static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<Scalar,typename SrcXprType::Scalar> &)
+  {
+    dst._assignProduct(src, -1, 1);
+  }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_TRIANGULARMATRIX_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/TernaryFunctors.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixVector.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
new file mode 100644
index 0000000..b2bb0c2
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
@@ -0,0 +1,1034 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MACROS_H
+#define EIGEN_MACROS_H
+
+#define EIGEN_WORLD_VERSION 3
+#define EIGEN_MAJOR_VERSION 3
+#define EIGEN_MINOR_VERSION 7
+
+#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
+                                      (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
+                                                                 EIGEN_MINOR_VERSION>=z))))
+
+// Compiler identification, EIGEN_COMP_*
+
+/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
+#ifdef __GNUC__
+  #define EIGEN_COMP_GNUC 1
+#else
+  #define EIGEN_COMP_GNUC 0
+#endif
+
+/// \internal EIGEN_COMP_CLANG set to major+minor version (e.g., 307 for clang 3.7) if the compiler is clang
+#if defined(__clang__)
+  #define EIGEN_COMP_CLANG (__clang_major__*100+__clang_minor__)
+#else
+  #define EIGEN_COMP_CLANG 0
+#endif
+
+
+/// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm
+#if defined(__llvm__)
+  #define EIGEN_COMP_LLVM 1
+#else
+  #define EIGEN_COMP_LLVM 0
+#endif
+
+/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel compiler, 0 otherwise
+#if defined(__INTEL_COMPILER)
+  #define EIGEN_COMP_ICC __INTEL_COMPILER
+#else
+  #define EIGEN_COMP_ICC 0
+#endif
+
+/// \internal EIGEN_COMP_MINGW set to 1 if the compiler is mingw
+#if defined(__MINGW32__)
+  #define EIGEN_COMP_MINGW 1
+#else
+  #define EIGEN_COMP_MINGW 0
+#endif
+
+/// \internal EIGEN_COMP_SUNCC set to 1 if the compiler is Solaris Studio
+#if defined(__SUNPRO_CC)
+  #define EIGEN_COMP_SUNCC 1
+#else
+  #define EIGEN_COMP_SUNCC 0
+#endif
+
+/// \internal EIGEN_COMP_MSVC set to _MSC_VER if the compiler is Microsoft Visual C++, 0 otherwise.
+#if defined(_MSC_VER)
+  #define EIGEN_COMP_MSVC _MSC_VER
+#else
+  #define EIGEN_COMP_MSVC 0
+#endif
+
+// For the record, here is a table summarizing the possible values for EIGEN_COMP_MSVC:
+//  name  ver   MSC_VER
+//  2008    9      1500
+//  2010   10      1600
+//  2012   11      1700
+//  2013   12      1800
+//  2015   14      1900
+//  "15"   15      1900
+
+/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC or clang-cl
+#if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC || EIGEN_COMP_LLVM || EIGEN_COMP_CLANG)
+  #define EIGEN_COMP_MSVC_STRICT _MSC_VER
+#else
+  #define EIGEN_COMP_MSVC_STRICT 0
+#endif
+
+/// \internal EIGEN_COMP_IBM set to 1 if the compiler is IBM XL C++
+#if defined(__IBMCPP__) || defined(__xlc__)
+  #define EIGEN_COMP_IBM 1
+#else
+  #define EIGEN_COMP_IBM 0
+#endif
+
+/// \internal EIGEN_COMP_PGI set to 1 if the compiler is Portland Group Compiler
+#if defined(__PGI)
+  #define EIGEN_COMP_PGI 1
+#else
+  #define EIGEN_COMP_PGI 0
+#endif
+
+/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
+#if defined(__CC_ARM) || defined(__ARMCC_VERSION)
+  #define EIGEN_COMP_ARM 1
+#else
+  #define EIGEN_COMP_ARM 0
+#endif
+
+/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
+#if defined(__EMSCRIPTEN__)
+  #define EIGEN_COMP_EMSCRIPTEN 1
+#else
+  #define EIGEN_COMP_EMSCRIPTEN 0
+#endif
+
+
+/// \internal EIGEN_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC, clang, mingw, etc.)
+#if EIGEN_COMP_GNUC && !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM || EIGEN_COMP_EMSCRIPTEN)
+  #define EIGEN_COMP_GNUC_STRICT 1
+#else
+  #define EIGEN_COMP_GNUC_STRICT 0
+#endif
+
+
+#if EIGEN_COMP_GNUC
+  #define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__==x && __GNUC_MINOR__>=y) || __GNUC__>x)
+  #define EIGEN_GNUC_AT_MOST(x,y)  ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
+  #define EIGEN_GNUC_AT(x,y)       ( __GNUC__==x && __GNUC_MINOR__==y )
+#else
+  #define EIGEN_GNUC_AT_LEAST(x,y) 0
+  #define EIGEN_GNUC_AT_MOST(x,y)  0
+  #define EIGEN_GNUC_AT(x,y)       0
+#endif
+
+// FIXME: could probably be removed as we do not support gcc 3.x anymore
+#if EIGEN_COMP_GNUC && (__GNUC__ <= 3)
+#define EIGEN_GCC3_OR_OLDER 1
+#else
+#define EIGEN_GCC3_OR_OLDER 0
+#endif
+
+
+// Architecture identification, EIGEN_ARCH_*
+
+#if defined(__x86_64__) || defined(_M_X64) || defined(__amd64)
+  #define EIGEN_ARCH_x86_64 1
+#else
+  #define EIGEN_ARCH_x86_64 0
+#endif
+
+#if defined(__i386__) || defined(_M_IX86) || defined(_X86_) || defined(__i386)
+  #define EIGEN_ARCH_i386 1
+#else
+  #define EIGEN_ARCH_i386 0
+#endif
+
+#if EIGEN_ARCH_x86_64 || EIGEN_ARCH_i386
+  #define EIGEN_ARCH_i386_OR_x86_64 1
+#else
+  #define EIGEN_ARCH_i386_OR_x86_64 0
+#endif
+
+/// \internal EIGEN_ARCH_ARM set to 1 if the architecture is ARM
+#if defined(__arm__)
+  #define EIGEN_ARCH_ARM 1
+#else
+  #define EIGEN_ARCH_ARM 0
+#endif
+
+/// \internal EIGEN_ARCH_ARM64 set to 1 if the architecture is ARM64
+#if defined(__aarch64__)
+  #define EIGEN_ARCH_ARM64 1
+#else
+  #define EIGEN_ARCH_ARM64 0
+#endif
+
+#if EIGEN_ARCH_ARM || EIGEN_ARCH_ARM64
+  #define EIGEN_ARCH_ARM_OR_ARM64 1
+#else
+  #define EIGEN_ARCH_ARM_OR_ARM64 0
+#endif
+
+/// \internal EIGEN_ARCH_MIPS set to 1 if the architecture is MIPS
+#if defined(__mips__) || defined(__mips)
+  #define EIGEN_ARCH_MIPS 1
+#else
+  #define EIGEN_ARCH_MIPS 0
+#endif
+
+/// \internal EIGEN_ARCH_SPARC set to 1 if the architecture is SPARC
+#if defined(__sparc__) || defined(__sparc)
+  #define EIGEN_ARCH_SPARC 1
+#else
+  #define EIGEN_ARCH_SPARC 0
+#endif
+
+/// \internal EIGEN_ARCH_IA64 set to 1 if the architecture is Intel Itanium
+#if defined(__ia64__)
+  #define EIGEN_ARCH_IA64 1
+#else
+  #define EIGEN_ARCH_IA64 0
+#endif
+
+/// \internal EIGEN_ARCH_PPC set to 1 if the architecture is PowerPC
+#if defined(__powerpc__) || defined(__ppc__) || defined(_M_PPC)
+  #define EIGEN_ARCH_PPC 1
+#else
+  #define EIGEN_ARCH_PPC 0
+#endif
+
+
+
+// Operating system identification, EIGEN_OS_*
+
+/// \internal EIGEN_OS_UNIX set to 1 if the OS is a unix variant
+#if defined(__unix__) || defined(__unix)
+  #define EIGEN_OS_UNIX 1
+#else
+  #define EIGEN_OS_UNIX 0
+#endif
+
+/// \internal EIGEN_OS_LINUX set to 1 if the OS is based on Linux kernel
+#if defined(__linux__)
+  #define EIGEN_OS_LINUX 1
+#else
+  #define EIGEN_OS_LINUX 0
+#endif
+
+/// \internal EIGEN_OS_ANDROID set to 1 if the OS is Android
+// note: ANDROID is defined when using ndk_build, __ANDROID__ is defined when using a standalone toolchain.
+#if defined(__ANDROID__) || defined(ANDROID)
+  #define EIGEN_OS_ANDROID 1
+#else
+  #define EIGEN_OS_ANDROID 0
+#endif
+
+/// \internal EIGEN_OS_GNULINUX set to 1 if the OS is GNU Linux and not Linux-based OS (e.g., not android)
+#if defined(__gnu_linux__) && !(EIGEN_OS_ANDROID)
+  #define EIGEN_OS_GNULINUX 1
+#else
+  #define EIGEN_OS_GNULINUX 0
+#endif
+
+/// \internal EIGEN_OS_BSD set to 1 if the OS is a BSD variant
+#if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__OpenBSD__) || defined(__bsdi__) || defined(__DragonFly__)
+  #define EIGEN_OS_BSD 1
+#else
+  #define EIGEN_OS_BSD 0
+#endif
+
+/// \internal EIGEN_OS_MAC set to 1 if the OS is MacOS
+#if defined(__APPLE__)
+  #define EIGEN_OS_MAC 1
+#else
+  #define EIGEN_OS_MAC 0
+#endif
+
+/// \internal EIGEN_OS_QNX set to 1 if the OS is QNX
+#if defined(__QNX__)
+  #define EIGEN_OS_QNX 1
+#else
+  #define EIGEN_OS_QNX 0
+#endif
+
+/// \internal EIGEN_OS_WIN set to 1 if the OS is Windows based
+#if defined(_WIN32)
+  #define EIGEN_OS_WIN 1
+#else
+  #define EIGEN_OS_WIN 0
+#endif
+
+/// \internal EIGEN_OS_WIN64 set to 1 if the OS is Windows 64bits
+#if defined(_WIN64)
+  #define EIGEN_OS_WIN64 1
+#else
+  #define EIGEN_OS_WIN64 0
+#endif
+
+/// \internal EIGEN_OS_WINCE set to 1 if the OS is Windows CE
+#if defined(_WIN32_WCE)
+  #define EIGEN_OS_WINCE 1
+#else
+  #define EIGEN_OS_WINCE 0
+#endif
+
+/// \internal EIGEN_OS_CYGWIN set to 1 if the OS is Windows/Cygwin
+#if defined(__CYGWIN__)
+  #define EIGEN_OS_CYGWIN 1
+#else
+  #define EIGEN_OS_CYGWIN 0
+#endif
+
+/// \internal EIGEN_OS_WIN_STRICT set to 1 if the OS is really Windows and not some variants
+#if EIGEN_OS_WIN && !( EIGEN_OS_WINCE || EIGEN_OS_CYGWIN )
+  #define EIGEN_OS_WIN_STRICT 1
+#else
+  #define EIGEN_OS_WIN_STRICT 0
+#endif
+
+/// \internal EIGEN_OS_SUN set to 1 if the OS is SUN
+#if (defined(sun) || defined(__sun)) && !(defined(__SVR4) || defined(__svr4__))
+  #define EIGEN_OS_SUN 1
+#else
+  #define EIGEN_OS_SUN 0
+#endif
+
+/// \internal EIGEN_OS_SOLARIS set to 1 if the OS is Solaris
+#if (defined(sun) || defined(__sun)) && (defined(__SVR4) || defined(__svr4__))
+  #define EIGEN_OS_SOLARIS 1
+#else
+  #define EIGEN_OS_SOLARIS 0
+#endif
+
+
+
+#if EIGEN_GNUC_AT_MOST(4,3) && !EIGEN_COMP_CLANG
+  // see bug 89
+  #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0
+#else
+  #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 1
+#endif
+
+// This macro can be used to prevent from macro expansion, e.g.:
+//   std::max EIGEN_NOT_A_MACRO(a,b)
+#define EIGEN_NOT_A_MACRO
+
+#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::RowMajor
+#else
+#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::ColMajor
+#endif
+
+#ifndef EIGEN_DEFAULT_DENSE_INDEX_TYPE
+#define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t
+#endif
+
+// Cross compiler wrapper around LLVM's __has_builtin
+#ifdef __has_builtin
+#  define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
+#else
+#  define EIGEN_HAS_BUILTIN(x) 0
+#endif
+
+// A Clang feature extension to determine compiler features.
+// We use it to determine 'cxx_rvalue_references'
+#ifndef __has_feature
+# define __has_feature(x) 0
+#endif
+
+// Upperbound on the C++ version to use.
+// Expected values are 03, 11, 14, 17, etc.
+// By default, let's use an arbitrarily large C++ version.
+#ifndef EIGEN_MAX_CPP_VER
+#define EIGEN_MAX_CPP_VER 99
+#endif
+
+#if EIGEN_MAX_CPP_VER>=11 && (defined(__cplusplus) && (__cplusplus >= 201103L) || EIGEN_COMP_MSVC >= 1900)
+#define EIGEN_HAS_CXX11 1
+#else
+#define EIGEN_HAS_CXX11 0
+#endif
+
+
+// Do we support r-value references?
+#ifndef EIGEN_HAS_RVALUE_REFERENCES
+#if EIGEN_MAX_CPP_VER>=11 && \
+    (__has_feature(cxx_rvalue_references) || \
+    (defined(__cplusplus) && __cplusplus >= 201103L) || \
+    (EIGEN_COMP_MSVC >= 1600))
+  #define EIGEN_HAS_RVALUE_REFERENCES 1
+#else
+  #define EIGEN_HAS_RVALUE_REFERENCES 0
+#endif
+#endif
+
+// Does the compiler support C99?
+#ifndef EIGEN_HAS_C99_MATH
+#if EIGEN_MAX_CPP_VER>=11 && \
+    ((defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901))       \
+  || (defined(__GNUC__) && defined(_GLIBCXX_USE_C99)) \
+  || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)))
+  #define EIGEN_HAS_C99_MATH 1
+#else
+  #define EIGEN_HAS_C99_MATH 0
+#endif
+#endif
+
+// Does the compiler support result_of?
+#ifndef EIGEN_HAS_STD_RESULT_OF
+#if EIGEN_MAX_CPP_VER>=11 && ((__has_feature(cxx_lambdas) || (defined(__cplusplus) && __cplusplus >= 201103L)))
+#define EIGEN_HAS_STD_RESULT_OF 1
+#else
+#define EIGEN_HAS_STD_RESULT_OF 0
+#endif
+#endif
+
+// Does the compiler support variadic templates?
+#ifndef EIGEN_HAS_VARIADIC_TEMPLATES
+#if EIGEN_MAX_CPP_VER>=11 && (__cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900) \
+  && (!defined(__NVCC__) || !EIGEN_ARCH_ARM_OR_ARM64 || (EIGEN_CUDACC_VER >= 80000) )
+    // ^^ Disable the use of variadic templates when compiling with versions of nvcc older than 8.0 on ARM devices:
+    //    this prevents nvcc from crashing when compiling Eigen on Tegra X1
+#define EIGEN_HAS_VARIADIC_TEMPLATES 1
+#else
+#define EIGEN_HAS_VARIADIC_TEMPLATES 0
+#endif
+#endif
+
+// Does the compiler fully support const expressions? (as in c++14)
+#ifndef EIGEN_HAS_CONSTEXPR
+
+#ifdef __CUDACC__
+// Const expressions are supported provided that c++11 is enabled and we're using either clang or nvcc 7.5 or above
+#if EIGEN_MAX_CPP_VER>=14 && (__cplusplus > 199711L && (EIGEN_COMP_CLANG || EIGEN_CUDACC_VER >= 70500))
+  #define EIGEN_HAS_CONSTEXPR 1
+#endif
+#elif EIGEN_MAX_CPP_VER>=14 && (__has_feature(cxx_relaxed_constexpr) || (defined(__cplusplus) && __cplusplus >= 201402L) || \
+  (EIGEN_GNUC_AT_LEAST(4,8) && (__cplusplus > 199711L)))
+#define EIGEN_HAS_CONSTEXPR 1
+#endif
+
+#ifndef EIGEN_HAS_CONSTEXPR
+#define EIGEN_HAS_CONSTEXPR 0
+#endif
+
+#endif
+
+// Does the compiler support C++11 math?
+// Let's be conservative and enable the default C++11 implementation only if we are sure it exists
+#ifndef EIGEN_HAS_CXX11_MATH
+  #if EIGEN_MAX_CPP_VER>=11 && ((__cplusplus > 201103L) || (__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC)  \
+      && (EIGEN_ARCH_i386_OR_x86_64) && (EIGEN_OS_GNULINUX || EIGEN_OS_WIN_STRICT || EIGEN_OS_MAC))
+    #define EIGEN_HAS_CXX11_MATH 1
+  #else
+    #define EIGEN_HAS_CXX11_MATH 0
+  #endif
+#endif
+
+// Does the compiler support proper C++11 containers?
+#ifndef EIGEN_HAS_CXX11_CONTAINERS
+  #if    EIGEN_MAX_CPP_VER>=11 && \
+         ((__cplusplus > 201103L) \
+      || ((__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_ICC>=1400)) \
+      || EIGEN_COMP_MSVC >= 1900)
+    #define EIGEN_HAS_CXX11_CONTAINERS 1
+  #else
+    #define EIGEN_HAS_CXX11_CONTAINERS 0
+  #endif
+#endif
+
+// Does the compiler support C++11 noexcept?
+#ifndef EIGEN_HAS_CXX11_NOEXCEPT
+  #if    EIGEN_MAX_CPP_VER>=11 && \
+         (__has_feature(cxx_noexcept) \
+      || (__cplusplus > 201103L) \
+      || ((__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_ICC>=1400)) \
+      || EIGEN_COMP_MSVC >= 1900)
+    #define EIGEN_HAS_CXX11_NOEXCEPT 1
+  #else
+    #define EIGEN_HAS_CXX11_NOEXCEPT 0
+  #endif
+#endif
+
+/** Allows to disable some optimizations which might affect the accuracy of the result.
+  * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
+  * They currently include:
+  *   - single precision ArrayBase::sin() and ArrayBase::cos() for SSE and AVX vectorization.
+  */
+#ifndef EIGEN_FAST_MATH
+#define EIGEN_FAST_MATH 1
+#endif
+
+#define EIGEN_DEBUG_VAR(x) std::cerr << #x << " = " << x << std::endl;
+
+// concatenate two tokens
+#define EIGEN_CAT2(a,b) a ## b
+#define EIGEN_CAT(a,b) EIGEN_CAT2(a,b)
+
+#define EIGEN_COMMA ,
+
+// convert a token to a string
+#define EIGEN_MAKESTRING2(a) #a
+#define EIGEN_MAKESTRING(a) EIGEN_MAKESTRING2(a)
+
+// EIGEN_STRONG_INLINE is a stronger version of the inline, using __forceinline on MSVC,
+// but it still doesn't use GCC's always_inline. This is useful in (common) situations where MSVC needs forceinline
+// but GCC is still doing fine with just inline.
+#ifndef EIGEN_STRONG_INLINE
+#if EIGEN_COMP_MSVC || EIGEN_COMP_ICC
+#define EIGEN_STRONG_INLINE __forceinline
+#else
+#define EIGEN_STRONG_INLINE inline
+#endif
+#endif
+
+// EIGEN_ALWAYS_INLINE is the stronget, it has the effect of making the function inline and adding every possible
+// attribute to maximize inlining. This should only be used when really necessary: in particular,
+// it uses __attribute__((always_inline)) on GCC, which most of the time is useless and can severely harm compile times.
+// FIXME with the always_inline attribute,
+// gcc 3.4.x and 4.1 reports the following compilation error:
+//   Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval<Derived> Eigen::MatrixBase<Scalar, Derived>::eval() const'
+//    : function body not available
+//   See also bug 1367
+#if EIGEN_GNUC_AT_LEAST(4,2)
+#define EIGEN_ALWAYS_INLINE __attribute__((always_inline)) inline
+#else
+#define EIGEN_ALWAYS_INLINE EIGEN_STRONG_INLINE
+#endif
+
+#if EIGEN_COMP_GNUC
+#define EIGEN_DONT_INLINE __attribute__((noinline))
+#elif EIGEN_COMP_MSVC
+#define EIGEN_DONT_INLINE __declspec(noinline)
+#else
+#define EIGEN_DONT_INLINE
+#endif
+
+#if EIGEN_COMP_GNUC
+#define EIGEN_PERMISSIVE_EXPR __extension__
+#else
+#define EIGEN_PERMISSIVE_EXPR
+#endif
+
+// this macro allows to get rid of linking errors about multiply defined functions.
+//  - static is not very good because it prevents definitions from different object files to be merged.
+//           So static causes the resulting linked executable to be bloated with multiple copies of the same function.
+//  - inline is not perfect either as it unwantedly hints the compiler toward inlining the function.
+#define EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+#define EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS inline
+
+#ifdef NDEBUG
+# ifndef EIGEN_NO_DEBUG
+#  define EIGEN_NO_DEBUG
+# endif
+#endif
+
+// eigen_plain_assert is where we implement the workaround for the assert() bug in GCC <= 4.3, see bug 89
+#ifdef EIGEN_NO_DEBUG
+  #define eigen_plain_assert(x)
+#else
+  #if EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO
+    namespace Eigen {
+    namespace internal {
+    inline bool copy_bool(bool b) { return b; }
+    }
+    }
+    #define eigen_plain_assert(x) assert(x)
+  #else
+    // work around bug 89
+    #include <cstdlib>   // for abort
+    #include <iostream>  // for std::cerr
+
+    namespace Eigen {
+    namespace internal {
+    // trivial function copying a bool. Must be EIGEN_DONT_INLINE, so we implement it after including Eigen headers.
+    // see bug 89.
+    namespace {
+    EIGEN_DONT_INLINE bool copy_bool(bool b) { return b; }
+    }
+    inline void assert_fail(const char *condition, const char *function, const char *file, int line)
+    {
+      std::cerr << "assertion failed: " << condition << " in function " << function << " at " << file << ":" << line << std::endl;
+      abort();
+    }
+    }
+    }
+    #define eigen_plain_assert(x) \
+      do { \
+        if(!Eigen::internal::copy_bool(x)) \
+          Eigen::internal::assert_fail(EIGEN_MAKESTRING(x), __PRETTY_FUNCTION__, __FILE__, __LINE__); \
+      } while(false)
+  #endif
+#endif
+
+// eigen_assert can be overridden
+#ifndef eigen_assert
+#define eigen_assert(x) eigen_plain_assert(x)
+#endif
+
+#ifdef EIGEN_INTERNAL_DEBUGGING
+#define eigen_internal_assert(x) eigen_assert(x)
+#else
+#define eigen_internal_assert(x)
+#endif
+
+#ifdef EIGEN_NO_DEBUG
+#define EIGEN_ONLY_USED_FOR_DEBUG(x) EIGEN_UNUSED_VARIABLE(x)
+#else
+#define EIGEN_ONLY_USED_FOR_DEBUG(x)
+#endif
+
+#ifndef EIGEN_NO_DEPRECATED_WARNING
+  #if EIGEN_COMP_GNUC
+    #define EIGEN_DEPRECATED __attribute__((deprecated))
+  #elif EIGEN_COMP_MSVC
+    #define EIGEN_DEPRECATED __declspec(deprecated)
+  #else
+    #define EIGEN_DEPRECATED
+  #endif
+#else
+  #define EIGEN_DEPRECATED
+#endif
+
+#if EIGEN_COMP_GNUC
+#define EIGEN_UNUSED __attribute__((unused))
+#else
+#define EIGEN_UNUSED
+#endif
+
+// Suppresses 'unused variable' warnings.
+namespace Eigen {
+  namespace internal {
+    template<typename T> EIGEN_DEVICE_FUNC void ignore_unused_variable(const T&) {}
+  }
+}
+#define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var);
+
+#if !defined(EIGEN_ASM_COMMENT)
+  #if EIGEN_COMP_GNUC && (EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64)
+    #define EIGEN_ASM_COMMENT(X)  __asm__("#" X)
+  #else
+    #define EIGEN_ASM_COMMENT(X)
+  #endif
+#endif
+
+
+//------------------------------------------------------------------------------------------
+// Static and dynamic alignment control
+//
+// The main purpose of this section is to define EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES
+// as the maximal boundary in bytes on which dynamically and statically allocated data may be alignment respectively.
+// The values of EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES can be specified by the user. If not,
+// a default value is automatically computed based on architecture, compiler, and OS.
+//
+// This section also defines macros EIGEN_ALIGN_TO_BOUNDARY(N) and the shortcuts EIGEN_ALIGN{8,16,32,_MAX}
+// to be used to declare statically aligned buffers.
+//------------------------------------------------------------------------------------------
+
+
+/* EIGEN_ALIGN_TO_BOUNDARY(n) forces data to be n-byte aligned. This is used to satisfy SIMD requirements.
+ * However, we do that EVEN if vectorization (EIGEN_VECTORIZE) is disabled,
+ * so that vectorization doesn't affect binary compatibility.
+ *
+ * If we made alignment depend on whether or not EIGEN_VECTORIZE is defined, it would be impossible to link
+ * vectorized and non-vectorized code.
+ */
+#if (defined __CUDACC__)
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __align__(n)
+#elif EIGEN_COMP_GNUC || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
+#elif EIGEN_COMP_MSVC
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __declspec(align(n))
+#elif EIGEN_COMP_SUNCC
+  // FIXME not sure about this one:
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
+#else
+  #error Please tell me what is the equivalent of __attribute__((aligned(n))) for your compiler
+#endif
+
+// If the user explicitly disable vectorization, then we also disable alignment
+#if defined(EIGEN_DONT_VECTORIZE)
+  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 0
+#elif defined(EIGEN_VECTORIZE_AVX512)
+  // 64 bytes static alignmeent is preferred only if really required
+  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 64
+#elif defined(__AVX__)
+  // 32 bytes static alignmeent is preferred only if really required
+  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 32
+#else
+  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
+#endif
+
+
+// EIGEN_MIN_ALIGN_BYTES defines the minimal value for which the notion of explicit alignment makes sense
+#define EIGEN_MIN_ALIGN_BYTES 16
+
+// Defined the boundary (in bytes) on which the data needs to be aligned. Note
+// that unless EIGEN_ALIGN is defined and not equal to 0, the data may not be
+// aligned at all regardless of the value of this #define.
+
+#if (defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN))  && defined(EIGEN_MAX_STATIC_ALIGN_BYTES) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
+#error EIGEN_MAX_STATIC_ALIGN_BYTES and EIGEN_DONT_ALIGN[_STATICALLY] are both defined with EIGEN_MAX_STATIC_ALIGN_BYTES!=0. Use EIGEN_MAX_STATIC_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN_STATICALLY.
+#endif
+
+// EIGEN_DONT_ALIGN_STATICALLY and EIGEN_DONT_ALIGN are deprectated
+// They imply EIGEN_MAX_STATIC_ALIGN_BYTES=0
+#if defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN)
+  #ifdef EIGEN_MAX_STATIC_ALIGN_BYTES
+    #undef EIGEN_MAX_STATIC_ALIGN_BYTES
+  #endif
+  #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
+#endif
+
+#ifndef EIGEN_MAX_STATIC_ALIGN_BYTES
+
+  // Try to automatically guess what is the best default value for EIGEN_MAX_STATIC_ALIGN_BYTES
+
+  // 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
+  // 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
+  // enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
+  // certain common platform (compiler+architecture combinations) to avoid these problems.
+  // Only static alignment is really problematic (relies on nonstandard compiler extensions),
+  // try to keep heap alignment even when we have to disable static alignment.
+  #if EIGEN_COMP_GNUC && !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64)
+  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
+  #elif EIGEN_ARCH_ARM_OR_ARM64 && EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_MOST(4, 6)
+  // Old versions of GCC on ARM, at least 4.4, were once seen to have buggy static alignment support.
+  // Not sure which version fixed it, hopefully it doesn't affect 4.7, which is still somewhat in use.
+  // 4.8 and newer seem definitely unaffected.
+  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
+  #else
+  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 0
+  #endif
+
+  // static alignment is completely disabled with GCC 3, Sun Studio, and QCC/QNX
+  #if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT \
+  && !EIGEN_GCC3_OR_OLDER \
+  && !EIGEN_COMP_SUNCC \
+  && !EIGEN_OS_QNX
+    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 1
+  #else
+    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 0
+  #endif
+
+  #if EIGEN_ARCH_WANTS_STACK_ALIGNMENT
+    #define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+  #else
+    #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
+  #endif
+
+#endif
+
+// If EIGEN_MAX_ALIGN_BYTES is defined, then it is considered as an upper bound for EIGEN_MAX_ALIGN_BYTES
+#if defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES<EIGEN_MAX_STATIC_ALIGN_BYTES
+#undef EIGEN_MAX_STATIC_ALIGN_BYTES
+#define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
+#endif
+
+#if EIGEN_MAX_STATIC_ALIGN_BYTES==0 && !defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
+  #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+#endif
+
+// At this stage, EIGEN_MAX_STATIC_ALIGN_BYTES>0 is the true test whether we want to align arrays on the stack or not.
+// It takes into account both the user choice to explicitly enable/disable alignment (by settting EIGEN_MAX_STATIC_ALIGN_BYTES)
+// and the architecture config (EIGEN_ARCH_WANTS_STACK_ALIGNMENT).
+// Henceforth, only EIGEN_MAX_STATIC_ALIGN_BYTES should be used.
+
+
+// Shortcuts to EIGEN_ALIGN_TO_BOUNDARY
+#define EIGEN_ALIGN8  EIGEN_ALIGN_TO_BOUNDARY(8)
+#define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16)
+#define EIGEN_ALIGN32 EIGEN_ALIGN_TO_BOUNDARY(32)
+#define EIGEN_ALIGN64 EIGEN_ALIGN_TO_BOUNDARY(64)
+#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
+#define EIGEN_ALIGN_MAX EIGEN_ALIGN_TO_BOUNDARY(EIGEN_MAX_STATIC_ALIGN_BYTES)
+#else
+#define EIGEN_ALIGN_MAX
+#endif
+
+
+// Dynamic alignment control
+
+#if defined(EIGEN_DONT_ALIGN) && defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES>0
+#error EIGEN_MAX_ALIGN_BYTES and EIGEN_DONT_ALIGN are both defined with EIGEN_MAX_ALIGN_BYTES!=0. Use EIGEN_MAX_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN.
+#endif
+
+#ifdef EIGEN_DONT_ALIGN
+  #ifdef EIGEN_MAX_ALIGN_BYTES
+    #undef EIGEN_MAX_ALIGN_BYTES
+  #endif
+  #define EIGEN_MAX_ALIGN_BYTES 0
+#elif !defined(EIGEN_MAX_ALIGN_BYTES)
+  #define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+#endif
+
+#if EIGEN_IDEAL_MAX_ALIGN_BYTES > EIGEN_MAX_ALIGN_BYTES
+#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+#else
+#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
+#endif
+
+
+#ifndef EIGEN_UNALIGNED_VECTORIZE
+#define EIGEN_UNALIGNED_VECTORIZE 1
+#endif
+
+//----------------------------------------------------------------------
+
+
+#ifdef EIGEN_DONT_USE_RESTRICT_KEYWORD
+  #define EIGEN_RESTRICT
+#endif
+#ifndef EIGEN_RESTRICT
+  #define EIGEN_RESTRICT __restrict
+#endif
+
+#ifndef EIGEN_STACK_ALLOCATION_LIMIT
+// 131072 == 128 KB
+#define EIGEN_STACK_ALLOCATION_LIMIT 131072
+#endif
+
+#ifndef EIGEN_DEFAULT_IO_FORMAT
+#ifdef EIGEN_MAKING_DOCS
+// format used in Eigen's documentation
+// needed to define it here as escaping characters in CMake add_definition's argument seems very problematic.
+#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(3, 0, " ", "\n", "", "")
+#else
+#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat()
+#endif
+#endif
+
+// just an empty macro !
+#define EIGEN_EMPTY
+
+#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC < 1900 || EIGEN_CUDACC_VER>0)
+  // for older MSVC versions, as well as 1900 && CUDA 8, using the base operator is sufficient (cf Bugs 1000, 1324)
+  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
+    using Base::operator =;
+#elif EIGEN_COMP_CLANG // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)
+  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
+    using Base::operator =; \
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) { Base::operator=(other); return *this; } \
+    template <typename OtherDerived> \
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase<OtherDerived>& other) { Base::operator=(other.derived()); return *this; }
+#else
+  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
+    using Base::operator =; \
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) \
+    { \
+      Base::operator=(other); \
+      return *this; \
+    }
+#endif
+
+
+/**
+ * \internal
+ * \brief Macro to explicitly define the default copy constructor.
+ * This is necessary, because the implicit definition is deprecated if the copy-assignment is overridden.
+ */
+#if EIGEN_HAS_CXX11
+#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default;
+#else
+#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS)
+#endif
+
+
+/** \internal
+ * \brief Macro to manually inherit assignment operators.
+ * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined.
+ * With C++11 or later this also default-implements the copy-constructor
+ */
+#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived)  \
+    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived)
+
+/** \internal
+ * \brief Macro to manually define default constructors and destructors.
+ * This is necessary when the copy constructor is re-defined.
+ * For empty helper classes this should usually be protected, to avoid accidentally creating empty objects.
+ *
+ * Hiding the default destructor lead to problems in C++03 mode together with boost::multiprecision
+ */
+#if EIGEN_HAS_CXX11
+#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived)  \
+    EIGEN_DEVICE_FUNC Derived() = default; \
+    EIGEN_DEVICE_FUNC ~Derived() = default;
+#else
+#define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived)  \
+    EIGEN_DEVICE_FUNC Derived() {}; \
+    /* EIGEN_DEVICE_FUNC ~Derived() {}; */
+#endif
+
+
+/**
+* Just a side note. Commenting within defines works only by documenting
+* behind the object (via '!<'). Comments cannot be multi-line and thus
+* we have these extra long lines. What is confusing doxygen over here is
+* that we use '\' and basically have a bunch of typedefs with their
+* documentation in a single line.
+**/
+
+#define EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) \
+  typedef typename Eigen::internal::traits<Derived>::Scalar Scalar; /*!< \brief Numeric type, e.g. float, double, int or std::complex<float>. */ \
+  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; /*!< \brief The underlying numeric type for composed scalar types. \details In cases where Scalar is e.g. std::complex<T>, T were corresponding to RealScalar. */ \
+  typedef typename Base::CoeffReturnType CoeffReturnType; /*!< \brief The return type for coefficient access. \details Depending on whether the object allows direct coefficient access (e.g. for a MatrixXd), this type is either 'const Scalar&' or simply 'Scalar' for objects that do not allow direct coefficient access. */ \
+  typedef typename Eigen::internal::ref_selector<Derived>::type Nested; \
+  typedef typename Eigen::internal::traits<Derived>::StorageKind StorageKind; \
+  typedef typename Eigen::internal::traits<Derived>::StorageIndex StorageIndex; \
+  enum { RowsAtCompileTime = Eigen::internal::traits<Derived>::RowsAtCompileTime, \
+        ColsAtCompileTime = Eigen::internal::traits<Derived>::ColsAtCompileTime, \
+        Flags = Eigen::internal::traits<Derived>::Flags, \
+        SizeAtCompileTime = Base::SizeAtCompileTime, \
+        MaxSizeAtCompileTime = Base::MaxSizeAtCompileTime, \
+        IsVectorAtCompileTime = Base::IsVectorAtCompileTime }; \
+  using Base::derived; \
+  using Base::const_cast_derived;
+
+
+// FIXME Maybe the EIGEN_DENSE_PUBLIC_INTERFACE could be removed as importing PacketScalar is rarely needed
+#define EIGEN_DENSE_PUBLIC_INTERFACE(Derived) \
+  EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) \
+  typedef typename Base::PacketScalar PacketScalar;
+
+
+#define EIGEN_PLAIN_ENUM_MIN(a,b) (((int)a <= (int)b) ? (int)a : (int)b)
+#define EIGEN_PLAIN_ENUM_MAX(a,b) (((int)a >= (int)b) ? (int)a : (int)b)
+
+// EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
+// followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
+// finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
+#define EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) (((int)a == 0 || (int)b == 0) ? 0 \
+                           : ((int)a == 1 || (int)b == 1) ? 1 \
+                           : ((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \
+                           : ((int)a <= (int)b) ? (int)a : (int)b)
+
+// EIGEN_SIZE_MIN_PREFER_FIXED is a variant of EIGEN_SIZE_MIN_PREFER_DYNAMIC comparing MaxSizes. The difference is that finite values
+// now have priority over Dynamic, so that min(3, Dynamic) gives 3. Indeed, whatever the actual value is
+// (between 0 and 3), it is not more than 3.
+#define EIGEN_SIZE_MIN_PREFER_FIXED(a,b)  (((int)a == 0 || (int)b == 0) ? 0 \
+                           : ((int)a == 1 || (int)b == 1) ? 1 \
+                           : ((int)a == Dynamic && (int)b == Dynamic) ? Dynamic \
+                           : ((int)a == Dynamic) ? (int)b \
+                           : ((int)b == Dynamic) ? (int)a \
+                           : ((int)a <= (int)b) ? (int)a : (int)b)
+
+// see EIGEN_SIZE_MIN_PREFER_DYNAMIC. No need for a separate variant for MaxSizes here.
+#define EIGEN_SIZE_MAX(a,b) (((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \
+                           : ((int)a >= (int)b) ? (int)a : (int)b)
+
+#define EIGEN_LOGICAL_XOR(a,b) (((a) || (b)) && !((a) && (b)))
+
+#define EIGEN_IMPLIES(a,b) (!(a) || (b))
+
+// the expression type of a standard coefficient wise binary operation
+#define EIGEN_CWISE_BINARY_RETURN_TYPE(LHS,RHS,OPNAME) \
+    CwiseBinaryOp< \
+      EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)< \
+          typename internal::traits<LHS>::Scalar, \
+          typename internal::traits<RHS>::Scalar \
+      >, \
+      const LHS, \
+      const RHS \
+    >
+
+#define EIGEN_MAKE_CWISE_BINARY_OP(METHOD,OPNAME) \
+  template<typename OtherDerived> \
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,OPNAME) \
+  (METHOD)(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const \
+  { \
+    return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,OPNAME)(derived(), other.derived()); \
+  }
+
+#define EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,TYPEA,TYPEB) \
+  (Eigen::internal::has_ReturnType<Eigen::ScalarBinaryOpTraits<TYPEA,TYPEB,EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,OPNAME),_op)<TYPEA,TYPEB>  > >::value)
+
+#define EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(EXPR,SCALAR,OPNAME) \
+  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)<typename internal::traits<EXPR>::Scalar,SCALAR>, const EXPR, \
+                const typename internal::plain_constant_type<EXPR,SCALAR>::type>
+
+#define EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(SCALAR,EXPR,OPNAME) \
+  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)<SCALAR,typename internal::traits<EXPR>::Scalar>, \
+                const typename internal::plain_constant_type<EXPR,SCALAR>::type, const EXPR>
+
+// Workaround for MSVC 2010 (see ML thread "patch with compile for for MSVC 2010")
+#if EIGEN_COMP_MSVC_STRICT<=1600
+#define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) typename internal::enable_if<true,X>::type
+#else
+#define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) X
+#endif
+
+#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME) \
+  template <typename T> EIGEN_DEVICE_FUNC inline \
+  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type,OPNAME))\
+  (METHOD)(const T& scalar) const { \
+    typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type PromotedT; \
+    return EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,PromotedT,OPNAME)(derived(), \
+           typename internal::plain_constant_type<Derived,PromotedT>::type(derived().rows(), derived().cols(), internal::scalar_constant_op<PromotedT>(scalar))); \
+  }
+
+#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD,OPNAME) \
+  template <typename T> EIGEN_DEVICE_FUNC inline friend \
+  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type,Derived,OPNAME)) \
+  (METHOD)(const T& scalar, const StorageBaseType& matrix) { \
+    typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type PromotedT; \
+    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedT,Derived,OPNAME)( \
+           typename internal::plain_constant_type<Derived,PromotedT>::type(matrix.derived().rows(), matrix.derived().cols(), internal::scalar_constant_op<PromotedT>(scalar)), matrix.derived()); \
+  }
+
+#define EIGEN_MAKE_SCALAR_BINARY_OP(METHOD,OPNAME) \
+  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD,OPNAME) \
+  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME)
+
+
+#ifdef EIGEN_EXCEPTIONS
+#  define EIGEN_THROW_X(X) throw X
+#  define EIGEN_THROW throw
+#  define EIGEN_TRY try
+#  define EIGEN_CATCH(X) catch (X)
+#else
+#  ifdef __CUDA_ARCH__
+#    define EIGEN_THROW_X(X) asm("trap;")
+#    define EIGEN_THROW asm("trap;")
+#  else
+#    define EIGEN_THROW_X(X) std::abort()
+#    define EIGEN_THROW std::abort()
+#  endif
+#  define EIGEN_TRY if (true)
+#  define EIGEN_CATCH(X) else
+#endif
+
+
+#if EIGEN_HAS_CXX11_NOEXCEPT
+#   define EIGEN_INCLUDE_TYPE_TRAITS
+#   define EIGEN_NOEXCEPT noexcept
+#   define EIGEN_NOEXCEPT_IF(x) noexcept(x)
+#   define EIGEN_NO_THROW noexcept(true)
+#   define EIGEN_EXCEPTION_SPEC(X) noexcept(false)
+#else
+#   define EIGEN_NOEXCEPT
+#   define EIGEN_NOEXCEPT_IF(x)
+#   define EIGEN_NO_THROW throw()
+#   if EIGEN_COMP_MSVC
+      // MSVC does not support exception specifications (warning C4290),
+      // and they are deprecated in c++11 anyway.
+#     define EIGEN_EXCEPTION_SPEC(X) throw()
+#   else
+#     define EIGEN_EXCEPTION_SPEC(X) throw(X)
+#   endif
+#endif
+
+#endif // EIGEN_MACROS_H

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h

wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h

diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
new file mode 100644
index 0000000..bf424a0
--- /dev/null
+++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
@@ -0,0 +1,824 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_XPRHELPER_H
+#define EIGEN_XPRHELPER_H
+
+// just a workaround because GCC seems to not really like empty structs
+// FIXME: gcc 4.3 generates bad code when strict-aliasing is enabled
+// so currently we simply disable this optimization for gcc 4.3
+#if EIGEN_COMP_GNUC && !EIGEN_GNUC_AT(4,3)
+  #define EIGEN_EMPTY_STRUCT_CTOR(X) \
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE X() {} \
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE X(const X& ) {}
+#else
+  #define EIGEN_EMPTY_STRUCT_CTOR(X)
+#endif
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename IndexDest, typename IndexSrc>
+EIGEN_DEVICE_FUNC
+inline IndexDest convert_index(const IndexSrc& idx) {
+  // for sizeof(IndexDest)>=sizeof(IndexSrc) compilers should be able to optimize this away:
+  eigen_internal_assert(idx <= NumTraits<IndexDest>::highest() && "Index value to big for target type");
+  return IndexDest(idx);
+}
+
+
+// promote_scalar_arg is an helper used in operation between an expression and a scalar, like:
+//    expression * scalar
+// Its role is to determine how the type T of the scalar operand should be promoted given the scalar type ExprScalar of the given expression.
+// The IsSupported template parameter must be provided by the caller as: internal::has_ReturnType<ScalarBinaryOpTraits<ExprScalar,T,op> >::value using the proper order for ExprScalar and T.
+// Then the logic is as follows:
+//  - if the operation is natively supported as defined by IsSupported, then the scalar type is not promoted, and T is returned.
+//  - otherwise, NumTraits<ExprScalar>::Literal is returned if T is implicitly convertible to NumTraits<ExprScalar>::Literal AND that this does not imply a float to integer conversion.
+//  - otherwise, ExprScalar is returned if T is implicitly convertible to ExprScalar AND that this does not imply a float to integer conversion.
+//  - In all other cases, the promoted type is not defined, and the respective operation is thus invalid and not available (SFINAE).
+template<typename ExprScalar,typename T, bool IsSupported>
+struct promote_scalar_arg;
+
+template<typename S,typename T>
+struct promote_scalar_arg<S,T,true>
+{
+  typedef T type;
+};
+
+// Recursively check safe conversion to PromotedType, and then ExprScalar if they are different.
+template<typename ExprScalar,typename T,typename PromotedType,
+  bool ConvertibleToLiteral = internal::is_convertible<T,PromotedType>::value,
+  bool IsSafe = NumTraits<T>::IsInteger || !NumTraits<PromotedType>::IsInteger>
+struct promote_scalar_arg_unsupported;
+
+// Start recursion with NumTraits<ExprScalar>::Literal
+template<typename S,typename T>
+struct promote_scalar_arg<S,T,false> : promote_scalar_arg_unsupported<S,T,typename NumTraits<S>::Literal> {};
+
+// We found a match!
+template<typename S,typename T, typename PromotedType>
+struct promote_scalar_arg_unsupported<S,T,PromotedType,true,true>
+{
+  typedef PromotedType type;
+};
+
+// No match, but no real-to-integer issues, and ExprScalar and current PromotedType are different,
+// so let's try to promote to ExprScalar
+template<typename ExprScalar,typename T, typename PromotedType>
+struct promote_scalar_arg_unsupported<ExprScalar,T,PromotedType,false,true>
+   : promote_scalar_arg_unsupported<ExprScalar,T,ExprScalar>
+{};
+
+// Unsafe real-to-integer, let's stop.
+template<typename S,typename T, typename PromotedType, bool ConvertibleToLiteral>
+struct promote_scalar_arg_unsupported<S,T,PromotedType,ConvertibleToLiteral,false> {};
+
+// T is not even convertible to ExprScalar, let's stop.
+template<typename S,typename T>
+struct promote_scalar_arg_unsupported<S,T,S,false,true> {};
+
+//classes inheriting no_assignment_operator don't generate a default operator=.
+class no_assignment_operator
+{
+  private:
+    no_assignment_operator& operator=(const no_assignment_operator&);
+  protected:
+    EIGEN_DEFAULT_COPY_CONSTRUCTOR(no_assignment_operator)
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(no_assignment_operator)
+};
+
+/** \internal return the index type with the largest number of bits */
+template<typename I1, typename I2>
+struct promote_index_type
+{
+  typedef typename conditional<(sizeof(I1)<sizeof(I2)), I2, I1>::type type;
+};
+
+/** \internal If the template parameter Value is Dynamic, this class is just a wrapper around a T variable that
+  * can be accessed using value() and setValue().
+  * Otherwise, this class is an empty structure and value() just returns the template parameter Value.
+  */
+template<typename T, int Value> class variable_if_dynamic
+{
+  public:
+    EIGEN_EMPTY_STRUCT_CTOR(variable_if_dynamic)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
+    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T value() { return T(Value); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {}
+};
+
+template<typename T> class variable_if_dynamic<T, Dynamic>
+{
+    T m_value;
+    EIGEN_DEVICE_FUNC variable_if_dynamic() { eigen_assert(false); }
+  public:
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value) : m_value(value) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T value() const { return m_value; }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
+};
+
+/** \internal like variable_if_dynamic but for DynamicIndex
+  */
+template<typename T, int Value> class variable_if_dynamicindex
+{
+  public:
+    EIGEN_EMPTY_STRUCT_CTOR(variable_if_dynamicindex)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
+    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T value() { return T(Value); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {}
+};
+
+template<typename T> class variable_if_dynamicindex<T, DynamicIndex>
+{
+    T m_value;
+    EIGEN_DEVICE_FUNC variable_if_dynamicindex() { eigen_assert(false); }
+  public:
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T value) : m_value(value) {}
+    EIGEN_DEVICE_FUNC T EIGEN_STRONG_INLINE value() const { return m_value; }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
+};
+
+template<typename T> struct functor_traits
+{
+  enum
+  {
+    Cost = 10,
+    PacketAccess = false,
+    IsRepeatable = false
+  };
+};
+
+template<typename T> struct packet_traits;
+
+template<typename T> struct unpacket_traits
+{
+  typedef T type;
+  typedef T half;
+  enum
+  {
+    size = 1,
+    alignment = 1
+  };
+};
+
+template<int Size, typename PacketType,
+         bool Stop = Size==Dynamic || (Size%unpacket_traits<PacketType>::size)==0 || is_same<PacketType,typename unpacket_traits<PacketType>::half>::value>
+struct find_best_packet_helper;
+
+template< int Size, typename PacketType>
+struct find_best_packet_helper<Size,PacketType,true>
+{
+  typedef PacketType type;
+};
+
+template<int Size, typename PacketType>
+struct find_best_packet_helper<Size,PacketType,false>
+{
+  typedef typename find_best_packet_helper<Size,typename unpacket_traits<PacketType>::half>::type type;
+};
+
+template<typename T, int Size>
+struct find_best_packet
+{
+  typedef typename find_best_packet_helper<Size,typename packet_traits<T>::type>::type type;
+};
+
+#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
+template<int ArrayBytes, int AlignmentBytes,
+         bool Match     =  bool((ArrayBytes%AlignmentBytes)==0),
+         bool TryHalf   =  bool(EIGEN_MIN_ALIGN_BYTES<AlignmentBytes) >
+struct compute_default_alignment_helper
+{
+  enum { value = 0 };
+};
+
+template<int ArrayBytes, int AlignmentBytes, bool TryHalf>
+struct compute_default_alignment_helper<ArrayBytes, AlignmentBytes, true, TryHalf> // Match
+{
+  enum { value = AlignmentBytes };
+};
+
+template<int ArrayBytes, int AlignmentBytes>
+struct compute_default_alignment_helper<ArrayBytes, AlignmentBytes, false, true> // Try-half
+{
+  // current packet too large, try with an half-packet
+  enum { value = compute_default_alignment_helper<ArrayBytes, AlignmentBytes/2>::value };
+};
+#else
+// If static alignment is disabled, no need to bother.
+// This also avoids a division by zero in "bool Match =  bool((ArrayBytes%AlignmentBytes)==0)"
+template<int ArrayBytes, int AlignmentBytes>
+struct compute_default_alignment_helper
+{
+  enum { value = 0 };
+};
+#endif
+
+template<typename T, int Size> struct compute_default_alignment {
+  enum { value = compute_default_alignment_helper<Size*sizeof(T),EIGEN_MAX_STATIC_ALIGN_BYTES>::value };
+};
+
+template<typename T> struct compute_default_alignment<T,Dynamic> {
+  enum { value = EIGEN_MAX_ALIGN_BYTES };
+};
+
+template<typename _Scalar, int _Rows, int _Cols,
+         int _Options = AutoAlign |
+                          ( (_Rows==1 && _Cols!=1) ? RowMajor
+                          : (_Cols==1 && _Rows!=1) ? ColMajor
+                          : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
+         int _MaxRows = _Rows,
+         int _MaxCols = _Cols
+> class make_proper_matrix_type
+{
+    enum {
+      IsColVector = _Cols==1 && _Rows!=1,
+      IsRowVector = _Rows==1 && _Cols!=1,
+      Options = IsColVector ? (_Options | ColMajor) & ~RowMajor
+              : IsRowVector ? (_Options | RowMajor) & ~ColMajor
+              : _Options
+    };
+  public:
+    typedef Matrix<_Scalar, _Rows, _Cols, Options, _MaxRows, _MaxCols> type;
+};
+
+template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+class compute_matrix_flags
+{
+    enum { row_major_bit = Options&RowMajor ? RowMajorBit : 0 };
+  public:
+    // FIXME currently we still have to handle DirectAccessBit at the expression level to handle DenseCoeffsBase<>
+    // and then propagate this information to the evaluator's flags.
+    // However, I (Gael) think that DirectAccessBit should only matter at the evaluation stage.
+    enum { ret = DirectAccessBit | LvalueBit | NestByRefBit | row_major_bit };
+};
+
+template<int _Rows, int _Cols> struct size_at_compile_time
+{
+  enum { ret = (_Rows==Dynamic || _Cols==Dynamic) ? Dynamic : _Rows * _Cols };
+};
+
+template<typename XprType> struct size_of_xpr_at_compile_time
+{
+  enum { ret = size_at_compile_time<traits<XprType>::RowsAtCompileTime,traits<XprType>::ColsAtCompileTime>::ret };
+};
+
+/* plain_matrix_type : the difference from eval is that plain_matrix_type is always a plain matrix type,
+ * whereas eval is a const reference in the case of a matrix
+ */
+
+template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct plain_matrix_type;
+template<typename T, typename BaseClassType, int Flags> struct plain_matrix_type_dense;
+template<typename T> struct plain_matrix_type<T,Dense>
+{
+  typedef typename plain_matrix_type_dense<T,typename traits<T>::XprKind, traits<T>::Flags>::type type;
+};
+template<typename T> struct plain_matrix_type<T,DiagonalShape>
+{
+  typedef typename T::PlainObject type;
+};
+
+template<typename T, int Flags> struct plain_matrix_type_dense<T,MatrixXpr,Flags>
+{
+  typedef Matrix<typename traits<T>::Scalar,
+                traits<T>::RowsAtCompileTime,
+                traits<T>::ColsAtCompileTime,
+                AutoAlign | (Flags&RowMajorBit ? RowMajor : ColMajor),
+                traits<T>::MaxRowsAtCompileTime,
+                traits<T>::MaxColsAtCompileTime
+          > type;
+};
+
+template<typename T, int Flags> struct plain_matrix_type_dense<T,ArrayXpr,Flags>
+{
+  typedef Array<typename traits<T>::Scalar,
+                traits<T>::RowsAtCompileTime,
+                traits<T>::ColsAtCompileTime,
+                AutoAlign | (Flags&RowMajorBit ? RowMajor : ColMajor),
+                traits<T>::MaxRowsAtCompileTime,
+                traits<T>::MaxColsAtCompileTime
+          > type;
+};
+
+/* eval : the return type of eval(). For matrices, this is just a const reference
+ * in order to avoid a useless copy
+ */
+
+template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct eval;
+
+template<typename T> struct eval<T,Dense>
+{
+  typedef typename plain_matrix_type<T>::type type;
+//   typedef typename T::PlainObject type;
+//   typedef T::Matrix<typename traits<T>::Scalar,
+//                 traits<T>::RowsAtCompileTime,
+//                 traits<T>::ColsAtCompileTime,
+//                 AutoAlign | (traits<T>::Flags&RowMajorBit ? RowMajor : ColMajor),
+//                 traits<T>::MaxRowsAtCompileTime,
+//                 traits<T>::MaxColsAtCompileTime
+//           > type;
+};
+
+template<typename T> struct eval<T,DiagonalShape>
+{
+  typedef typename plain_matrix_type<T>::type type;
+};
+
+// for matrices, no need to evaluate, just use a const reference to avoid a useless copy
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+struct eval<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, Dense>
+{
+  typedef const Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& type;
+};
+
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+struct eval<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, Dense>
+{
+  typedef const Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& type;
+};
+
+
+/* similar to plain_matrix_type, but using the evaluator's Flags */
+template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct plain_object_eval;
+
+template<typename T>
+struct plain_object_eval<T,Dense>
+{
+  typedef typename plain_matrix_type_dense<T,typename traits<T>::XprKind, evaluator<T>::Flags>::type type;
+};
+
+
+/* plain_matrix_type_column_major : same as plain_matrix_type but guaranteed to be column-major
+ */
+template<typename T> struct plain_matrix_type_column_major
+{
+  enum { Rows = traits<T>::RowsAtCompileTime,
+         Cols = traits<T>::ColsAtCompileTime,
+         MaxRows = traits<T>::MaxRowsAtCompileTime,
+         MaxCols = traits<T>::MaxColsAtCompileTime
+  };
+  typedef Matrix<typename traits<T>::Scalar,
+                Rows,
+                Cols,
+                (MaxRows==1&&MaxCols!=1) ? RowMajor : ColMajor,
+                MaxRows,
+                MaxCols
+          > type;
+};
+
+/* plain_matrix_type_row_major : same as plain_matrix_type but guaranteed to be row-major
+ */
+template<typename T> struct plain_matrix_type_row_major
+{
+  enum { Rows = traits<T>::RowsAtCompileTime,
+         Cols = traits<T>::ColsAtCompileTime,
+         MaxRows = traits<T>::MaxRowsAtCompileTime,
+         MaxCols = traits<T>::MaxColsAtCompileTime
+  };
+  typedef Matrix<typename traits<T>::Scalar,
+                Rows,
+                Cols,
+                (MaxCols==1&&MaxRows!=1) ? RowMajor : ColMajor,
+                MaxRows,
+                MaxCols
+          > type;
+};
+
+/** \internal The reference selector for template expressions. The idea is that we don't
+  * need to use references for expressions since they are light weight proxy
+  * objects which should generate no copying overhead. */
+template <typename T>
+struct ref_selector
+{
+  typedef typename conditional<
+    bool(traits<T>::Flags & NestByRefBit),
+    T const&,
+    const T
+  >::type type;
+  
+  typedef typename conditional<
+    bool(traits<T>::Flags & NestByRefBit),
+    T &,
+    T
+  >::type non_const_type;
+};
+
+/** \internal Adds the const qualifier on the value-type of T2 if and only if T1 is a const type */
+template<typename T1, typename T2>
+struct transfer_constness
+{
+  typedef typename conditional<
+    bool(internal::is_const<T1>::value),
+    typename internal::add_const_on_value_type<T2>::type,
+    T2
+  >::type type;
+};
+
+
+// However, we still need a mechanism to detect whether an expression which is evaluated multiple time
+// has to be evaluated into a temporary.
+// That's the purpose of this new nested_eval helper:
+/** \internal Determines how a given expression should be nested when evaluated multiple times.
+  * For example, when you do a * (b+c), Eigen will determine how the expression b+c should be
+  * evaluated into the bigger product expression. The choice is between nesting the expression b+c as-is, or
+  * evaluating that expression b+c into a temporary variable d, and nest d so that the resulting expression is
+  * a*d. Evaluating can be beneficial for example if every coefficient access in the resulting expression causes
+  * many coefficient accesses in the nested expressions -- as is the case with matrix product for example.
+  *
+  * \tparam T the type of the expression being nested.
+  * \tparam n the number of coefficient accesses in the nested expression for each coefficient access in the bigger expression.
+  * \tparam PlainObject the type of the temporary if needed.
+  */
+template<typename T, int n, typename PlainObject = typename plain_object_eval<T>::type> struct nested_eval
+{
+  enum {
+    ScalarReadCost = NumTraits<typename traits<T>::Scalar>::ReadCost,
+    CoeffReadCost = evaluator<T>::CoeffReadCost,  // NOTE What if an evaluator evaluate itself into a tempory?
+                                                  //      Then CoeffReadCost will be small (e.g., 1) but we still have to evaluate, especially if n>1.
+                                                  //      This situation is already taken care by the EvalBeforeNestingBit flag, which is turned ON
+                                                  //      for all evaluator creating a temporary. This flag is then propagated by the parent evaluators.
+                                                  //      Another solution could be to count the number of temps?
+    NAsInteger = n == Dynamic ? HugeCost : n,
+    CostEval   = (NAsInteger+1) * ScalarReadCost + CoeffReadCost,
+    CostNoEval = NAsInteger * CoeffReadCost,
+    Evaluate = (int(evaluator<T>::Flags) & EvalBeforeNestingBit) || (int(CostEval) < int(CostNoEval))
+  };
+
+  typedef typename conditional<Evaluate, PlainObject, typename ref_selector<T>::type>::type type;
+};
+
+template<typename T>
+EIGEN_DEVICE_FUNC
+inline T* const_cast_ptr(const T* ptr)
+{
+  return const_cast<T*>(ptr);
+}
+
+template<typename Derived, typename XprKind = typename traits<Derived>::XprKind>
+struct dense_xpr_base
+{
+  /* dense_xpr_base should only ever be used on dense expressions, thus falling either into the MatrixXpr or into the ArrayXpr cases */
+};
+
+template<typename Derived>
+struct dense_xpr_base<Derived, MatrixXpr>
+{
+  typedef MatrixBase<Derived> type;
+};
+
+template<typename Derived>
+struct dense_xpr_base<Derived, ArrayXpr>
+{
+  typedef ArrayBase<Derived> type;
+};
+
+template<typename Derived, typename XprKind = typename traits<Derived>::XprKind, typename StorageKind = typename traits<Derived>::StorageKind>
+struct generic_xpr_base;
+
+template<typename Derived, typename XprKind>
+struct generic_xpr_base<Derived, XprKind, Dense>
+{
+  typedef typename dense_xpr_base<Derived,XprKind>::type type;
+};
+
+template<typename XprType, typename CastType> struct cast_return_type
+{
+  typedef typename XprType::Scalar CurrentScalarType;
+  typedef typename remove_all<CastType>::type _CastType;
+  typedef typename _CastType::Scalar NewScalarType;
+  typedef typename conditional<is_same<CurrentScalarType,NewScalarType>::value,
+                              const XprType&,CastType>::type type;
+};
+
+template <typename A, typename B> struct promote_storage_type;
+
+template <typename A> struct promote_storage_type<A,A>
+{
+  typedef A ret;
+};
+template <typename A> struct promote_storage_type<A, const A>
+{
+  typedef A ret;
+};
+template <typename A> struct promote_storage_type<const A, A>
+{
+  typedef A ret;
+};
+
+/** \internal Specify the "storage kind" of applying a coefficient-wise
+  * binary operations between two expressions of kinds A and B respectively.
+  * The template parameter Functor permits to specialize the resulting storage kind wrt to
+  * the functor.
+  * The default rules are as follows:
+  * \code
+  * A      op A      -> A
+  * A      op dense  -> dense
+  * dense  op B      -> dense
+  * sparse op dense  -> sparse
+  * dense  op sparse -> sparse
+  * \endcode
+  */
+template <typename A, typename B, typename Functor> struct cwise_promote_storage_type;
+
+template <typename A, typename Functor>                   struct cwise_promote_storage_type<A,A,Functor>                                      { typedef A      ret; };
+template <typename Functor>                               struct cwise_promote_storage_type<Dense,Dense,Functor>                              { typedef Dense  ret; };
+template <typename A, typename Functor>                   struct cwise_promote_storage_type<A,Dense,Functor>                                  { typedef Dense  ret; };
+template <typename B, typename Functor>                   struct cwise_promote_storage_type<Dense,B,Functor>                                  { typedef Dense  ret; };
+template <typename Functor>                               struct cwise_promote_storage_type<Sparse,Dense,Functor>                             { typedef Sparse ret; };
+template <typename Functor>                               struct cwise_promote_storage_type<Dense,Sparse,Functor>                             { typedef Sparse ret; };
+
+template <typename LhsKind, typename RhsKind, int LhsOrder, int RhsOrder> struct cwise_promote_storage_order {
+  enum { value = LhsOrder };
+};
+
+template <typename LhsKind, int LhsOrder, int RhsOrder>   struct cwise_promote_storage_order<LhsKind,Sparse,LhsOrder,RhsOrder>                { enum { value = RhsOrder }; };
+template <typename RhsKind, int LhsOrder, int RhsOrder>   struct cwise_promote_storage_order<Sparse,RhsKind,LhsOrder,RhsOrder>                { enum { value = LhsOrder }; };
+template <int Order>                                      struct cwise_promote_storage_order<Sparse,Sparse,Order,Order>                       { enum { value = Order }; };
+
+
+/** \internal Specify the "storage kind" of multiplying an expression of kind A with kind B.
+  * The template parameter ProductTag permits to specialize the resulting storage kind wrt to
+  * some compile-time properties of the product: GemmProduct, GemvProduct, OuterProduct, InnerProduct.
+  * The default rules are as follows:
+  * \code
+  *  K * K            -> K
+  *  dense * K        -> dense
+  *  K * dense        -> dense
+  *  diag * K         -> K
+  *  K * diag         -> K
+  *  Perm * K         -> K
+  * K * Perm          -> K
+  * \endcode
+  */
+template <typename A, typename B, int ProductTag> struct product_promote_storage_type;
+
+template <typename A, int ProductTag> struct product_promote_storage_type<A,                  A,                  ProductTag> { typedef A     ret;};
+template <int ProductTag>             struct product_promote_storage_type<Dense,              Dense,              ProductTag> { typedef Dense ret;};
+template <typename A, int ProductTag> struct product_promote_storage_type<A,                  Dense,              ProductTag> { typedef Dense ret; };
+template <typename B, int ProductTag> struct product_promote_storage_type<Dense,              B,                  ProductTag> { typedef Dense ret; };
+
+template <typename A, int ProductTag> struct product_promote_storage_type<A,                  DiagonalShape,      ProductTag> { typedef A ret; };
+template <typename B, int ProductTag> struct product_promote_storage_type<DiagonalShape,      B,                  ProductTag> { typedef B ret; };
+template <int ProductTag>             struct product_promote_storage_type<Dense,              DiagonalShape,      ProductTag> { typedef Dense ret; };
+template <int ProductTag>             struct product_promote_storage_type<DiagonalShape,      Dense,              ProductTag> { typedef Dense ret; };
+
+template <typename A, int ProductTag> struct product_promote_storage_type<A,                  PermutationStorage, ProductTag> { typedef A ret; };
+template <typename B, int ProductTag> struct product_promote_storage_type<PermutationStorage, B,                  ProductTag> { typedef B ret; };
+template <int ProductTag>             struct product_promote_storage_type<Dense,              PermutationStorage, ProductTag> { typedef Dense ret; };
+template <int ProductTag>             struct product_promote_storage_type<PermutationStorage, Dense,              ProductTag> { typedef Dense ret; };
+
+/** \internal gives the plain matrix or array type to store a row/column/diagonal of a matrix type.
+  * \tparam Scalar optional parameter allowing to pass a different scalar type than the one of the MatrixType.
+  */
+template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
+struct plain_row_type
+{
+  typedef Matrix<Scalar, 1, ExpressionType::ColsAtCompileTime,
+                 ExpressionType::PlainObject::Options | RowMajor, 1, ExpressionType::MaxColsAtCompileTime> MatrixRowType;
+  typedef Array<Scalar, 1, ExpressionType::ColsAtCompileTime,
+                 ExpressionType::PlainObject::Options | RowMajor, 1, ExpressionType::MaxColsAtCompileTime> ArrayRowType;
+
+  typedef typename conditional<
+    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
+    MatrixRowType,
+    ArrayRowType 
+  >::type type;
+};
+
+template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
+struct plain_col_type
+{
+  typedef Matrix<Scalar, ExpressionType::RowsAtCompileTime, 1,
+                 ExpressionType::PlainObject::Options & ~RowMajor, ExpressionType::MaxRowsAtCompileTime, 1> MatrixColType;
+  typedef Array<Scalar, ExpressionType::RowsAtCompileTime, 1,
+                 ExpressionType::PlainObject::Options & ~RowMajor, ExpressionType::MaxRowsAtCompileTime, 1> ArrayColType;
+
+  typedef typename conditional<
+    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
+    MatrixColType,
+    ArrayColType 
+  >::type type;
+};
+
+template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
+struct plain_diag_type
+{
+  enum { diag_size = EIGEN_SIZE_MIN_PREFER_DYNAMIC(ExpressionType::RowsAtCompileTime, ExpressionType::ColsAtCompileTime),
+         max_diag_size = EIGEN_SIZE_MIN_PREFER_FIXED(ExpressionType::MaxRowsAtCompileTime, ExpressionType::MaxColsAtCompileTime)
+  };
+  typedef Matrix<Scalar, diag_size, 1, ExpressionType::PlainObject::Options & ~RowMajor, max_diag_size, 1> MatrixDiagType;
+  typedef Array<Scalar, diag_size, 1, ExpressionType::PlainObject::Options & ~RowMajor, max_diag_size, 1> ArrayDiagType;
+
+  typedef typename conditional<
+    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
+    MatrixDiagType,
+    ArrayDiagType 
+  >::type type;
+};
+
+template<typename Expr,typename Scalar = typename Expr::Scalar>
+struct plain_constant_type
+{
+  enum { Options = (traits<Expr>::Flags&RowMajorBit)?RowMajor:0 };
+
+  typedef Array<Scalar,  traits<Expr>::RowsAtCompileTime,   traits<Expr>::ColsAtCompileTime,
+                Options, traits<Expr>::MaxRowsAtCompileTime,traits<Expr>::MaxColsAtCompileTime> array_type;
+
+  typedef Matrix<Scalar,  traits<Expr>::RowsAtCompileTime,   traits<Expr>::ColsAtCompileTime,
+                 Options, traits<Expr>::MaxRowsAtCompileTime,traits<Expr>::MaxColsAtCompileTime> matrix_type;
+
+  typedef CwiseNullaryOp<scalar_constant_op<Scalar>, const typename conditional<is_same< typename traits<Expr>::XprKind, MatrixXpr >::value, matrix_type, array_type>::type > type;
+};
+
+template<typename ExpressionType>
+struct is_lvalue
+{
+  enum { value = (!bool(is_const<ExpressionType>::value)) &&
+                 bool(traits<ExpressionType>::Flags & LvalueBit) };
+};
+
+template<typename T> struct is_diagonal
+{ enum { ret = false }; };
+
+template<typename T> struct is_diagonal<DiagonalBase<T> >
+{ enum { ret = true }; };
+
+template<typename T> struct is_diagonal<DiagonalWrapper<T> >
+{ enum { ret = true }; };
+
+template<typename T, int S> struct is_diagonal<DiagonalMatrix<T,S> >
+{ enum { ret = true }; };
+
+template<typename S1, typename S2> struct glue_shapes;
+template<> struct glue_shapes<DenseShape,TriangularShape> { typedef TriangularShape type;  };
+
+template<typename T1, typename T2>
+bool is_same_dense(const T1 &mat1, const T2 &mat2, typename enable_if<has_direct_access<T1>::ret&&has_direct_access<T2>::ret, T1>::type * = 0)
+{
+  return (mat1.data()==mat2.data()) && (mat1.innerStride()==mat2.innerStride()) && (mat1.outerStride()==mat2.outerStride());
+}
+
+template<typename T1, typename T2>
+bool is_same_dense(const T1 &, const T2 &, typename enable_if<!(has_direct_access<T1>::ret&&has_direct_access<T2>::ret), T1>::type * = 0)
+{
+  return false;
+}
+
+// Internal helper defining the cost of a scalar division for the type T.
+// The default heuristic can be specialized for each scalar type and architecture.
+template<typename T,bool Vectorized=false,typename EnaleIf = void>
+struct scalar_div_cost {
+  enum { value = 8*NumTraits<T>::MulCost };
+};
+
+template<typename T,bool Vectorized>
+struct scalar_div_cost<std::complex<T>, Vectorized> {
+  enum { value = 2*scalar_div_cost<T>::value
+               + 6*NumTraits<T>::MulCost
+               + 3*NumTraits<T>::AddCost
+  };
+};
+
+
+template<bool Vectorized>
+struct scalar_div_cost<signed long,Vectorized,typename conditional<sizeof(long)==8,void,false_type>::type> { enum { value = 24 }; };
+template<bool Vectorized>
+struct scalar_div_cost<unsigned long,Vectorized,typename conditional<sizeof(long)==8,void,false_type>::type> { enum { value = 21 }; };
+
+
+#ifdef EIGEN_DEBUG_ASSIGN
+std::string demangle_traversal(int t)
+{
+  if(t==DefaultTraversal) return "DefaultTraversal";
+  if(t==LinearTraversal) return "LinearTraversal";
+  if(t==InnerVectorizedTraversal) return "InnerVectorizedTraversal";
+  if(t==LinearVectorizedTraversal) return "LinearVectorizedTraversal";
+  if(t==SliceVectorizedTraversal) return "SliceVectorizedTraversal";
+  return "?";
+}
+std::string demangle_unrolling(int t)
+{
+  if(t==NoUnrolling) return "NoUnrolling";
+  if(t==InnerUnrolling) return "InnerUnrolling";
+  if(t==CompleteUnrolling) return "CompleteUnrolling";
+  return "?";
+}
+std::string demangle_flags(int f)
+{
+  std::string res;
+  if(f&RowMajorBit)                 res += " | RowMajor";
+  if(f&PacketAccessBit)             res += " | Packet";
+  if(f&LinearAccessBit)             res += " | Linear";
+  if(f&LvalueBit)                   res += " | Lvalue";
+  if(f&DirectAccessBit)             res += " | Direct";
+  if(f&NestByRefBit)                res += " | NestByRef";
+  if(f&NoPreferredStorageOrderBit)  res += " | NoPreferredStorageOrderBit";
+  
+  return res;
+}
+#endif
+
+} // end namespace internal
+
+
+/** \class ScalarBinaryOpTraits
+  * \ingroup Core_Module
+  *
+  * \brief Determines whether the given binary operation of two numeric types is allowed and what the scalar return type is.
+  *
+  * This class permits to control the scalar return type of any binary operation performed on two different scalar types through (partial) template specializations.
+  *
+  * For instance, let \c U1, \c U2 and \c U3 be three user defined scalar types for which most operations between instances of \c U1 and \c U2 returns an \c U3.
+  * You can let %Eigen knows that by defining:
+    \code
+    template<typename BinaryOp>
+    struct ScalarBinaryOpTraits<U1,U2,BinaryOp> { typedef U3 ReturnType;  };
+    template<typename BinaryOp>
+    struct ScalarBinaryOpTraits<U2,U1,BinaryOp> { typedef U3 ReturnType;  };
+    \endcode
+  * You can then explicitly disable some particular operations to get more explicit error messages:
+    \code
+    template<>
+    struct ScalarBinaryOpTraits<U1,U2,internal::scalar_max_op<U1,U2> > {};
+    \endcode
+  * Or customize the return type for individual operation:
+    \code
+    template<>
+    struct ScalarBinaryOpTraits<U1,U2,internal::scalar_sum_op<U1,U2> > { typedef U1 ReturnType; };
+    \endcode
+  *
+  * By default, the following generic combinations are supported:
+  <table class="manual">
+  <tr><th>ScalarA</th><th>ScalarB</th><th>BinaryOp</th><th>ReturnType</th><th>Note</th></tr>
+  <tr            ><td>\c T </td><td>\c T </td><td>\c * </td><td>\c T </td><td></td></tr>
+  <tr class="alt"><td>\c NumTraits<T>::Real </td><td>\c T </td><td>\c * </td><td>\c T </td><td>Only if \c NumTraits<T>::IsComplex </td></tr>
+  <tr            ><td>\c T </td><td>\c NumTraits<T>::Real </td><td>\c * </td><td>\c T </td><td>Only if \c NumTraits<T>::IsComplex </td></tr>
+  </table>
+  *
+  * \sa CwiseBinaryOp
+  */
+template<typename ScalarA, typename ScalarB, typename BinaryOp=internal::scalar_product_op<ScalarA,ScalarB> >
+struct ScalarBinaryOpTraits
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  // for backward compatibility, use the hints given by the (deprecated) internal::scalar_product_traits class.
+  : internal::scalar_product_traits<ScalarA,ScalarB>
+#endif // EIGEN_PARSED_BY_DOXYGEN
+{};
+
+template<typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<T,T,BinaryOp>
+{
+  typedef T ReturnType;
+};
+
+template <typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<T, typename NumTraits<typename internal::enable_if<NumTraits<T>::IsComplex,T>::type>::Real, BinaryOp>
+{
+  typedef T ReturnType;
+};
+template <typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<typename NumTraits<typename internal::enable_if<NumTraits<T>::IsComplex,T>::type>::Real, T, BinaryOp>
+{
+  typedef T ReturnType;
+};
+
+// For Matrix * Permutation
+template<typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<T,void,BinaryOp>
+{
+  typedef T ReturnType;
+};
+
+// For Permutation * Matrix
+template<typename T, typename BinaryOp>
+struct ScalarBinaryOpTraits<void,T,BinaryOp>
+{
+  typedef T ReturnType;
+};
+
+// for Permutation*Permutation
+template<typename BinaryOp>
+struct ScalarBinaryOpTraits<void,void,BinaryOp>
+{
+  typedef void ReturnType;
+};
+
+// We require Lhs and Rhs to have "compatible" scalar types.
+// It is tempting to always allow mixing different types but remember that this is often impossible in the vectorized paths.
+// So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user tries to
+// add together a float matrix and a double matrix.
+#define EIGEN_CHECK_BINARY_COMPATIBILIY(BINOP,LHS,RHS) \
+  EIGEN_STATIC_ASSERT((Eigen::internal::has_ReturnType<ScalarBinaryOpTraits<LHS, RHS,BINOP> >::value), \
+    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+    
+} // end namespace Eigen
+
+#endif // EIGEN_XPRHELPER_H

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h

wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h

wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h

wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h

wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h

wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h

wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h b/wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h

wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h b/wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h

wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h b/wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h

wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h b/wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h

wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h b/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h

wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h b/wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h

wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h

wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h b/wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h

wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h b/wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h

wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h b/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h

wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h b/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h

wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h b/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h

wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h b/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h

wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h b/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h

wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h b/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h

wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h b/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h

wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h b/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/details.h

wpimath/src/main/native/eigeninclude/Eigen/src/misc/Image.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Image.h b/wpimath/src/main/native/eigeninclude/Eigen/src/misc/Image.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Image.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/misc/Image.h

wpimath/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h b/wpimath/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/misc/Kernel.h

wpimath/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h b/wpimath/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/misc/RealSvd2x2.h

wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h

wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h

wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h b/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h

wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h b/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseBinaryOps.h

wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h b/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h

wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h

wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h
rename to wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h

wpimath/src/main/native/include/drake/common/drake_assert.h

diff --git a/wpimath/src/main/native/include/drake/common/drake_assert.h b/wpimath/src/main/native/include/drake/common/drake_assert.h
new file mode 100644
index 0000000..21e7bd1
--- /dev/null
+++ b/wpimath/src/main/native/include/drake/common/drake_assert.h
@@ -0,0 +1,152 @@
+#pragma once
+
+#include <type_traits>
+
+/// @file
+/// Provides Drake's assertion implementation.  This is intended to be used
+/// both within Drake and by other software.  Drake's asserts can be armed
+/// and disarmed independently from the system-wide asserts.
+
+#ifdef DRAKE_DOXYGEN_CXX
+/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition)
+/// from the C++ system header @p <cassert>.  Unless Drake's assertions are
+/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT
+/// will evaluate @p condition and iff the value is false will trigger an
+/// assertion failure with a message showing at least the condition text,
+/// function name, file, and line.
+///
+/// By default, assertion failures will :abort() the program.  However, when
+/// using the pydrake python bindings, assertion failures will instead throw a
+/// C++ exception that causes a python SystemExit exception.
+///
+/// Assertions are enabled or disabled using the following pre-processor macros:
+///
+/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed.
+/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed.
+/// - If both macros are defined, then it is a compile-time error.
+/// - If neither are defined, then NDEBUG governs assertions as usual.
+///
+/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or
+/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed.
+///
+/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed`
+/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals.
+///
+/// One difference versus the standard @p assert(condition) is that the
+/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if
+/// Drake's assertions are disarmed.
+///
+/// Treat @p DRAKE_ASSERT like a statement -- it must always be used
+/// in block scope, and must always be followed by a semicolon.
+#define DRAKE_ASSERT(condition)
+/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this
+/// allows for guarding expensive assertion-checking subroutines using the same
+/// macros as stand-alone assertions.
+#define DRAKE_ASSERT_VOID(expression)
+/// Evaluates @p condition and iff the value is false will trigger an assertion
+/// failure with a message showing at least the condition text, function name,
+/// file, and line.
+#define DRAKE_DEMAND(condition)
+/// Silences a "no return value" compiler warning by calling a function that
+/// always raises an exception or aborts (i.e., a function marked noreturn).
+/// Only use this macro at a point where (1) a point in the code is truly
+/// unreachable, (2) the fact that it's unreachable is knowable from only
+/// reading the function itself (and not, e.g., some larger design invariant),
+/// and (3) there is a compiler warning if this macro were removed.  The most
+/// common valid use is with a switch-case-return block where all cases are
+/// accounted for but the enclosing function is supposed to return a value.  Do
+/// *not* use this macro as a "logic error" assertion; it should *only* be used
+/// to silence false positive warnings.  When in doubt, throw an exception
+/// manually instead of using this macro.
+#define DRAKE_UNREACHABLE()
+#else  //  DRAKE_DOXYGEN_CXX
+
+// Users should NOT set these; only this header should set them.
+#ifdef DRAKE_ASSERT_IS_ARMED
+# error Unexpected DRAKE_ASSERT_IS_ARMED defined.
+#endif
+#ifdef DRAKE_ASSERT_IS_DISARMED
+# error Unexpected DRAKE_ASSERT_IS_DISARMED defined.
+#endif
+
+// Decide whether Drake assertions are enabled.
+#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS)
+# error Conflicting assertion toggles.
+#elif defined(DRAKE_ENABLE_ASSERTS)
+# define DRAKE_ASSERT_IS_ARMED
+#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG)
+# define DRAKE_ASSERT_IS_DISARMED
+#else
+# define DRAKE_ASSERT_IS_ARMED
+#endif
+
+namespace drake {
+namespace internal {
+// Abort the program with an error message.
+[[noreturn]]
+void Abort(const char* condition, const char* func, const char* file, int line);
+// Report an assertion failure; will either Abort(...) or throw.
+[[noreturn]]
+void AssertionFailed(
+    const char* condition, const char* func, const char* file, int line);
+}  // namespace internal
+namespace assert {
+// Allows for specialization of how to bool-convert Conditions used in
+// assertions, in case they are not intrinsically convertible.  See
+// symbolic_formula.h for an example use.  This is a public interface to
+// extend; it is intended to be specialized by unusual Scalar types that
+// require special handling.
+template <typename Condition>
+struct ConditionTraits {
+  static constexpr bool is_valid = std::is_convertible<Condition, bool>::value;
+  static bool Evaluate(const Condition& value) {
+    return value;
+  }
+};
+}  // namespace assert
+}  // namespace drake
+
+#define DRAKE_UNREACHABLE()                                             \
+  ::drake::internal::Abort(                                             \
+      "Unreachable code was reached?!", __func__, __FILE__, __LINE__)
+
+#define DRAKE_DEMAND(condition)                                              \
+  do {                                                                       \
+    typedef ::drake::assert::ConditionTraits<                                \
+        typename std::remove_cv<decltype(condition)>::type> Trait;           \
+    static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
+    if (!Trait::Evaluate(condition)) {                                       \
+      ::drake::internal::AssertionFailed(                                    \
+           #condition, __func__, __FILE__, __LINE__);                        \
+    }                                                                        \
+  } while (0)
+
+#ifdef DRAKE_ASSERT_IS_ARMED
+// Assertions are enabled.
+namespace drake {
+constexpr bool kDrakeAssertIsArmed = true;
+constexpr bool kDrakeAssertIsDisarmed = false;
+}  // namespace drake
+# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition)
+# define DRAKE_ASSERT_VOID(expression) do {                     \
+    static_assert(                                              \
+        std::is_convertible<decltype(expression), void>::value, \
+        "Expression should be void.");                          \
+    expression;                                                 \
+  } while (0)
+#else
+// Assertions are disabled, so just typecheck the expression.
+namespace drake {
+constexpr bool kDrakeAssertIsArmed = false;
+constexpr bool kDrakeAssertIsDisarmed = true;
+}  // namespace drake
+# define DRAKE_ASSERT(condition) static_assert(                        \
+    ::drake::assert::ConditionTraits<                                  \
+        typename std::remove_cv<decltype(condition)>::type>::is_valid, \
+    "Condition should be bool-convertible.");
+# define DRAKE_ASSERT_VOID(expression) static_assert(           \
+    std::is_convertible<decltype(expression), void>::value,     \
+    "Expression should be void.")
+#endif
+
+#endif  // DRAKE_DOXYGEN_CXX

wpimath/src/main/native/include/drake/common/drake_assertion_error.h

diff --git a/wpimath/src/main/native/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/include/drake/common/drake_assertion_error.h
new file mode 100644
index 0000000..541b118
--- /dev/null
+++ b/wpimath/src/main/native/include/drake/common/drake_assertion_error.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <stdexcept>
+#include <string>
+
+namespace drake {
+namespace internal {
+
+/// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
+/// configured to throw.
+class assertion_error : public std::runtime_error {
+ public:
+  explicit assertion_error(const std::string& what_arg)
+      : std::runtime_error(what_arg) {}
+};
+
+}  // namespace internal
+}  // namespace drake

wpimath/src/main/native/include/drake/common/drake_copyable.h

diff --git a/wpimath/src/main/native/include/drake/common/drake_copyable.h b/wpimath/src/main/native/include/drake/common/drake_copyable.h
new file mode 100644
index 0000000..086f1f7
--- /dev/null
+++ b/wpimath/src/main/native/include/drake/common/drake_copyable.h
@@ -0,0 +1,112 @@
+#pragma once
+
+// ============================================================================
+// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this
+// file must be kept in sync!
+// ============================================================================
+
+/** @file
+Provides careful macros to selectively enable or disable the special member
+functions for copy-construction, copy-assignment, move-construction, and
+move-assignment.
+
+http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions
+
+When enabled via these macros, the `= default` implementation is provided.
+Code that needs custom copy or move functions should not use these macros.
+*/
+
+/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
+copy-construction, copy-assignment, move-construction, and move-assignment.
+Drake's Doxygen is customized to render the deletions in detail, with
+appropriate comments.  Invoke this this macro in the public section of the
+class declaration, e.g.:
+<pre>
+class Foo {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
+
+  // ...
+};
+</pre>
+*/
+#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)      \
+  Classname(const Classname&) = delete;                 \
+  void operator=(const Classname&) = delete;            \
+  Classname(Classname&&) = delete;                      \
+  void operator=(Classname&&) = delete;
+
+/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member
+functions for copy-construction, copy-assignment, move-construction, and
+move-assignment.  This macro should be used only when copy-construction and
+copy-assignment defaults are well-formed.  Note that the defaulted move
+functions could conceivably still be ill-formed, in which case they will
+effectively not be declared or used -- but because the copy constructor exists
+the type will still be MoveConstructible.  Drake's Doxygen is customized to
+render the functions in detail, with appropriate comments.  Invoke this this
+macro in the public section of the class declaration, e.g.:
+<pre>
+class Foo {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
+
+  // ...
+};
+</pre>
+*/
+#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
+  Classname(const Classname&) = default;                        \
+  Classname& operator=(const Classname&) = default;             \
+  Classname(Classname&&) = default;                             \
+  Classname& operator=(Classname&&) = default;                  \
+  /* Fails at compile-time if default-copy doesn't work. */     \
+  static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() {        \
+    (void) static_cast<Classname& (Classname::*)(               \
+        const Classname&)>(&Classname::operator=);              \
+  }
+
+/** DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN declares (but does not define) the
+special member functions for copy-construction, copy-assignment,
+move-construction, and move-assignment.  Drake's Doxygen is customized to
+render the declarations with appropriate comments.
+
+This is useful when paired with DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T
+to work around https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 whereby the
+declaration and definition must be split.  Once Drake no longer supports GCC
+versions prior to 6.3, this macro could be removed.
+
+Invoke this this macro in the public section of the class declaration, e.g.:
+<pre>
+template <typename T>
+class Foo {
+ public:
+  DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Foo)
+
+  // ...
+};
+DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Foo)
+</pre>
+*/
+#define DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
+  Classname(const Classname&);                                          \
+  Classname& operator=(const Classname&);                               \
+  Classname(Classname&&);                                               \
+  Classname& operator=(Classname&&);                                    \
+  /* Fails at compile-time if default-copy doesn't work. */             \
+  static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() {                \
+    (void) static_cast<Classname& (Classname::*)(                       \
+        const Classname&)>(&Classname::operator=);                      \
+  }
+
+/** Helper for DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN.  Provides defaulted
+definitions for the four special member functions of a templated class.
+*/
+#define DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Classname)      \
+  template <typename T>                                                 \
+  Classname<T>::Classname(const Classname<T>&) = default;               \
+  template <typename T>                                                 \
+  Classname<T>& Classname<T>::operator=(const Classname<T>&) = default; \
+  template <typename T>                                                 \
+  Classname<T>::Classname(Classname<T>&&) = default;                    \
+  template <typename T>                                                 \
+  Classname<T>& Classname<T>::operator=(Classname<T>&&) = default;

wpimath/src/main/native/include/drake/common/drake_throw.h

diff --git a/wpimath/src/main/native/include/drake/common/drake_throw.h b/wpimath/src/main/native/include/drake/common/drake_throw.h
new file mode 100644
index 0000000..ff42bb7
--- /dev/null
+++ b/wpimath/src/main/native/include/drake/common/drake_throw.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <type_traits>
+
+#include "drake/common/drake_assert.h"
+
+/// @file
+/// Provides a convenient wrapper to throw an exception when a condition is
+/// unmet.  This is similar to an assertion, but uses exceptions instead of
+/// ::abort(), and cannot be disabled.
+
+namespace drake {
+namespace internal {
+// Throw an error message.
+[[noreturn]]
+void Throw(const char* condition, const char* func, const char* file, int line);
+}  // namespace internal
+}  // namespace drake
+
+/// Evaluates @p condition and iff the value is false will throw an exception
+/// with a message showing at least the condition text, function name, file,
+/// and line.
+#define DRAKE_THROW_UNLESS(condition)                                        \
+  do {                                                                       \
+    typedef ::drake::assert::ConditionTraits<                                \
+        typename std::remove_cv<decltype(condition)>::type> Trait;           \
+    static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
+    if (!Trait::Evaluate(condition)) {                                       \
+      ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__);    \
+    }                                                                        \
+  } while (0)

wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h

diff --git a/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
new file mode 100644
index 0000000..b3f369c
--- /dev/null
+++ b/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Core>
+
+namespace drake {
+
+/// Returns true if and only if the two matrices are equal to within a certain
+/// absolute elementwise @p tolerance.  Special values (infinities, NaN, etc.)
+/// do not compare as equal elements.
+template <typename DerivedA, typename DerivedB>
+bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
+                            const Eigen::MatrixBase<DerivedB>& m2,
+                            double tolerance) {
+  return (
+      (m1.rows() == m2.rows()) &&
+      (m1.cols() == m2.cols()) &&
+      ((m1 - m2).template lpNorm<Eigen::Infinity>() <= tolerance));
+}
+
+/// Returns true if and only if a simple greedy search reveals a permutation
+/// of the columns of m2 to make the matrix equal to m1 to within a certain
+/// absolute elementwise @p tolerance. E.g., there exists a P such that
+/// <pre>
+///    forall i,j,  |m1 - m2*P|_{i,j} <= tolerance
+///    where P is a permutation matrix:
+///       P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
+/// </pre>
+/// Note: Returns false for matrices of different sizes.
+/// Note: The current implementation is O(n^2) in the number of columns.
+/// Note: In marginal cases (with similar but not identical columns) this
+/// algorithm can fail to find a permutation P even if it exists because it
+/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the
+/// pool. It is possible that other columns of m2 would also match m1(i) but
+/// that m2(j) is the only match possible for a later column of m1.
+template <typename DerivedA, typename DerivedB>
+bool IsApproxEqualAbsTolWithPermutedColumns(
+    const Eigen::MatrixBase<DerivedA>& m1,
+    const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
+  if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
+
+  std::vector<bool> available(m2.cols());
+  for (int i = 0; i < m2.cols(); i++) available[i] = true;
+
+  for (int i = 0; i < m1.cols(); i++) {
+    bool found_match = false;
+    for (int j = 0; j < m2.cols(); j++) {
+      if (available[j] &&
+          is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) {
+        found_match = true;
+        available[j] = false;
+        break;
+      }
+    }
+    if (!found_match) return false;
+  }
+  return true;
+}
+
+
+}  // namespace drake

wpimath/src/main/native/include/drake/common/never_destroyed.h

diff --git a/wpimath/src/main/native/include/drake/common/never_destroyed.h b/wpimath/src/main/native/include/drake/common/never_destroyed.h
new file mode 100644
index 0000000..2033fd0
--- /dev/null
+++ b/wpimath/src/main/native/include/drake/common/never_destroyed.h
@@ -0,0 +1,84 @@
+#pragma once
+
+#include <new>
+#include <type_traits>
+#include <utility>
+
+#include "drake/common/drake_copyable.h"
+
+namespace drake {
+
+/// Wraps an underlying type T such that its storage is a direct member field
+/// of this object (i.e., without any indirection into the heap), but *unlike*
+/// most member fields T's destructor is never invoked.
+///
+/// This is especially useful for function-local static variables that are not
+/// trivially destructable.  We shouldn't call their destructor at program exit
+/// because of the "indeterminate order of ... destruction" as mentioned in
+/// cppguide's
+/// <a href="https://drake.mit.edu/styleguide/cppguide.html#Static_and_Global_Variables">Static
+/// and Global Variables</a> section, but other solutions to this problem place
+///  the objects on the heap through an indirection.
+///
+/// Compared with other approaches, this mechanism more clearly describes the
+/// intent to readers, avoids "possible leak" warnings from memory-checking
+/// tools, and is probably slightly faster.
+///
+/// Example uses:
+///
+/// The singleton pattern:
+/// @code
+/// class Singleton {
+///  public:
+///   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton)
+///   static Singleton& getInstance() {
+///     static never_destroyed<Singleton> instance;
+///     return instance.access();
+///   }
+///  private:
+///   friend never_destroyed<Singleton>;
+///   Singleton() = default;
+/// };
+/// @endcode
+///
+/// A lookup table, created on demand the first time its needed, and then
+/// reused thereafter:
+/// @code
+/// enum class Foo { kBar, kBaz };
+/// Foo ParseFoo(const std::string& foo_string) {
+///   using Dict = std::unordered_map<std::string, Foo>;
+///   static const drake::never_destroyed<Dict> string_to_enum{
+///     std::initializer_list<Dict::value_type>{
+///       {"bar", Foo::kBar},
+///       {"baz", Foo::kBaz},
+///     }
+///   };
+///   return string_to_enum.access().at(foo_string);
+/// }
+/// @endcode
+//
+// The above examples are repeated in the unit test; keep them in sync.
+template <typename T>
+class never_destroyed {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed)
+
+  /// Passes the constructor arguments along to T using perfect forwarding.
+  template <typename... Args>
+  explicit never_destroyed(Args&&... args) {
+    // Uses "placement new" to construct a `T` in `storage_`.
+    new (&storage_) T(std::forward<Args>(args)...);
+  }
+
+  /// Does nothing.  Guaranteed!
+  ~never_destroyed() = default;
+
+  /// Returns the underlying T reference.
+  T& access() { return *reinterpret_cast<T*>(&storage_); }
+  const T& access() const { return *reinterpret_cast<const T*>(&storage_); }
+
+ private:
+  typename std::aligned_storage<sizeof(T), alignof(T)>::type storage_;
+};
+
+}  // namespace drake

wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h

diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
new file mode 100644
index 0000000..e45cdc8
--- /dev/null
+++ b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <cmath>
+#include <cstdlib>
+
+#include <Eigen/Core>
+
+namespace drake {
+namespace math {
+
+/// Computes the unique stabilizing solution X to the discrete-time algebraic
+/// Riccati equation:
+///
+/// \f[
+/// A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
+/// \f]
+///
+/// @throws std::runtime_error if Q is not positive semi-definite.
+/// @throws std::runtime_error if R is not positive definite.
+///
+/// Based on the Schur Vector approach outlined in this paper:
+/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
+/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
+///
+Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+    const Eigen::Ref<const Eigen::MatrixXd>& A,
+    const Eigen::Ref<const Eigen::MatrixXd>& B,
+    const Eigen::Ref<const Eigen::MatrixXd>& Q,
+    const Eigen::Ref<const Eigen::MatrixXd>& R);
+}  // namespace math
+}  // namespace drake
+

wpimath/src/main/native/include/frc/LinearFilter.h

diff --git a/wpimath/src/main/native/include/frc/LinearFilter.h b/wpimath/src/main/native/include/frc/LinearFilter.h
new file mode 100644
index 0000000..1fe0edc
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/LinearFilter.h
@@ -0,0 +1,196 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cassert>
+#include <cmath>
+#include <initializer_list>
+#include <vector>
+
+#include <wpi/ArrayRef.h>
+#include <wpi/circular_buffer.h>
+
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR
+ * filters are supported. Static factory methods are provided to create commonly
+ * used types of filters.
+ *
+ * Filters are of the form:<br>
+ *  y[n] = (b0 * x[n] + b1 * x[n-1] + … + bP * x[n-P]) -
+ *         (a0 * y[n-1] + a2 * y[n-2] + … + aQ * y[n-Q])
+ *
+ * Where:<br>
+ *  y[n] is the output at time "n"<br>
+ *  x[n] is the input at time "n"<br>
+ *  y[n-1] is the output from the LAST time step ("n-1")<br>
+ *  x[n-1] is the input from the LAST time step ("n-1")<br>
+ *  b0 … bP are the "feedforward" (FIR) gains<br>
+ *  a0 … aQ are the "feedback" (IIR) gains<br>
+ * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
+ *            convention in signal processing.
+ *
+ * What can linear filters do? Basically, they can filter, or diminish, the
+ * effects of undesirable input frequencies. High frequencies, or rapid changes,
+ * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
+ * filter smooths out the signal, reducing the impact of these high frequency
+ * components.  Likewise, a "high pass" filter gets rid of slow-moving signal
+ * components, letting you detect large changes more easily.
+ *
+ * Example FRC applications of filters:
+ *  - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
+ *    can do this faster in hardware)
+ *  - Smoothing out joystick input to prevent the wheels from slipping or the
+ *    robot from tipping
+ *  - Smoothing motor commands so that unnecessary strain isn't put on
+ *    electrical or mechanical components
+ *  - If you use clever gains, you can make a PID controller out of this class!
+ *
+ * For more on filters, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * Note 1: Calculate() should be called by the user on a known, regular period.
+ * You can use a Notifier for this or do it "inline" with code in a
+ * periodic function.
+ *
+ * Note 2: For ALL filters, gains are necessarily a function of frequency. If
+ * you make a filter that works well for you at, say, 100Hz, you will most
+ * definitely need to adjust the gains if you then want to run it at 200Hz!
+ * Combining this with Note 1 - the impetus is on YOU as a developer to make
+ * sure Calculate() gets called at the desired, constant frequency!
+ */
+template <class T>
+class LinearFilter {
+ public:
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feed forward" or FIR gains.
+   * @param fbGains The "feed back" or IIR gains.
+   */
+  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains)
+      : m_inputs(ffGains.size()),
+        m_outputs(fbGains.size()),
+        m_inputGains(ffGains),
+        m_outputGains(fbGains) {
+    static int instances = 0;
+    instances++;
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kFilter_Linear, 1);
+  }
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feed forward" or FIR gains.
+   * @param fbGains The "feed back" or IIR gains.
+   */
+  LinearFilter(std::initializer_list<double> ffGains,
+               std::initializer_list<double> fbGains)
+      : LinearFilter(wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
+                     wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
+
+  // Static methods to create commonly used filters
+  /**
+   * Creates a one-pole IIR low-pass filter of the form:<br>
+   *   y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
+   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+   *
+   * This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the
+   *                     user.
+   */
+  static LinearFilter<T> SinglePoleIIR(double timeConstant,
+                                       units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter(1.0 - gain, -gain);
+  }
+
+  /**
+   * Creates a first-order high-pass filter of the form:<br>
+   *   y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
+   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+   *
+   * This filter is stable for time constants greater than zero.
+   *
+   * @param timeConstant The discrete-time time constant in seconds.
+   * @param period       The period in seconds between samples taken by the
+   *                     user.
+   */
+  static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter({gain, -gain}, {-gain});
+  }
+
+  /**
+   * Creates a K-tap FIR moving average filter of the form:<br>
+   *   y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
+   *
+   * This filter is always stable.
+   *
+   * @param taps The number of samples to average over. Higher = smoother but
+   *             slower
+   */
+  static LinearFilter<T> MovingAverage(int taps) {
+    assert(taps > 0);
+
+    std::vector<double> gains(taps, 1.0 / taps);
+    return LinearFilter(gains, {});
+  }
+
+  /**
+   * Reset the filter state.
+   */
+  void Reset() {
+    m_inputs.reset();
+    m_outputs.reset();
+  }
+
+  /**
+   * Calculates the next value of the filter.
+   *
+   * @param input Current input value.
+   *
+   * @return The filtered value at this step
+   */
+  T Calculate(T input) {
+    T retVal = T(0.0);
+
+    // Rotate the inputs
+    m_inputs.push_front(input);
+
+    // Calculate the new value
+    for (size_t i = 0; i < m_inputGains.size(); i++) {
+      retVal += m_inputs[i] * m_inputGains[i];
+    }
+    for (size_t i = 0; i < m_outputGains.size(); i++) {
+      retVal -= m_outputs[i] * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    m_outputs.push_front(retVal);
+
+    return retVal;
+  }
+
+ private:
+  wpi::circular_buffer<T> m_inputs;
+  wpi::circular_buffer<T> m_outputs;
+  std::vector<double> m_inputGains;
+  std::vector<double> m_outputGains;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/MedianFilter.h

diff --git a/wpimath/src/main/native/include/frc/MedianFilter.h b/wpimath/src/main/native/include/frc/MedianFilter.h
new file mode 100644
index 0000000..3ccccbf
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/MedianFilter.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+#include <vector>
+
+#include <wpi/Algorithm.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing
+ * measurement noise, especially with processes that generate occasional,
+ * extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
+ * sensors).
+ */
+template <class T>
+class MedianFilter {
+ public:
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  T Calculate(T next) {
+    // Insert next value at proper point in sorted array
+    wpi::insert_sorted(m_orderedValues, next);
+
+    size_t curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.erase(std::find(m_orderedValues.begin(),
+                                      m_orderedValues.end(),
+                                      m_valueBuffer.pop_back()));
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.push_front(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues[curSize / 2];
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
+             2.0;
+    }
+  }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  void Reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.reset();
+  }
+
+ private:
+  wpi::circular_buffer<T> m_valueBuffer;
+  std::vector<T> m_orderedValues;
+  size_t m_size;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/StateSpaceUtil.h

diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
new file mode 100644
index 0000000..b461005
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -0,0 +1,309 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <cmath>
+#include <random>
+#include <type_traits>
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "Eigen/src/Eigenvalues/EigenSolver.h"
+#include "frc/geometry/Pose2d.h"
+
+namespace frc {
+namespace detail {
+
+template <int Rows, int Cols, typename Matrix, typename T, typename... Ts>
+void MatrixImpl(Matrix& result, T elem, Ts... elems) {
+  constexpr int count = Rows * Cols - (sizeof...(Ts) + 1);
+
+  result(count / Cols, count % Cols) = elem;
+  if constexpr (sizeof...(Ts) > 0) {
+    MatrixImpl<Rows, Cols>(result, elems...);
+  }
+}
+
+template <typename Matrix, typename T, typename... Ts>
+void CostMatrixImpl(Matrix& result, T elem, Ts... elems) {
+  result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2);
+  if constexpr (sizeof...(Ts) > 0) {
+    CostMatrixImpl(result, elems...);
+  }
+}
+
+template <typename Matrix, typename T, typename... Ts>
+void CovMatrixImpl(Matrix& result, T elem, Ts... elems) {
+  result(result.rows() - (sizeof...(Ts) + 1)) = std::pow(elem, 2);
+  if constexpr (sizeof...(Ts) > 0) {
+    CovMatrixImpl(result, elems...);
+  }
+}
+
+template <typename Matrix, typename T, typename... Ts>
+void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+  std::normal_distribution<> distr{0.0, elem};
+
+  result(result.rows() - (sizeof...(Ts) + 1)) = distr(gen);
+  if constexpr (sizeof...(Ts) > 0) {
+    WhiteNoiseVectorImpl(result, elems...);
+  }
+}
+
+template <int States, int Inputs>
+bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
+                        const Eigen::Matrix<double, States, Inputs>& B) {
+  Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es(A);
+
+  for (int i = 0; i < States; ++i) {
+    if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
+            es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
+        1) {
+      continue;
+    }
+
+    Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
+    E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
+                                             States>::Identity() -
+             A,
+        B;
+
+    Eigen::ColPivHouseholderQR<
+        Eigen::Matrix<std::complex<double>, States, States + Inputs>>
+        qr(E);
+    if (qr.rank() < States) {
+      return false;
+    }
+  }
+  return true;
+}
+
+}  // namespace detail
+
+/**
+ * Creates a matrix from the given list of elements.
+ *
+ * The elements of the matrix are filled in in row-major order.
+ *
+ * @param elems An array of elements in the matrix.
+ * @return A matrix containing the given elements.
+ */
+template <int Rows, int Cols, typename... Ts,
+          typename =
+              std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
+Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
+  static_assert(
+      sizeof...(elems) == Rows * Cols,
+      "Number of provided elements doesn't match matrix dimensionality");
+
+  Eigen::Matrix<double, Rows, Cols> result;
+  detail::MatrixImpl<Rows, Cols>(result, elems...);
+  return result;
+}
+
+/**
+ * Creates a cost matrix from the given vector for use with LQR.
+ *
+ * The cost matrix is constructed using Bryson's rule. The inverse square of
+ * each element in the input is taken and placed on the cost matrix diagonal.
+ *
+ * @param costs An array. For a Q matrix, its elements are the maximum allowed
+ *              excursions of the states from the reference. For an R matrix,
+ *              its elements are the maximum allowed excursions of the control
+ *              inputs from no actuation.
+ * @return State excursion or control effort cost matrix.
+ */
+template <typename... Ts, typename = std::enable_if_t<
+                              std::conjunction_v<std::is_same<double, Ts>...>>>
+Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
+    Ts... costs) {
+  Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
+  detail::CostMatrixImpl(result.diagonal(), costs...);
+  return result;
+}
+
+/**
+ * Creates a covariance matrix from the given vector for use with Kalman
+ * filters.
+ *
+ * Each element is squared and placed on the covariance matrix diagonal.
+ *
+ * @param stdDevs An array. For a Q matrix, its elements are the standard
+ *                deviations of each state from how the model behaves. For an R
+ *                matrix, its elements are the standard deviations for each
+ *                output measurement.
+ * @return Process noise or measurement noise covariance matrix.
+ */
+template <typename... Ts, typename = std::enable_if_t<
+                              std::conjunction_v<std::is_same<double, Ts>...>>>
+Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
+    Ts... stdDevs) {
+  Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
+  detail::CovMatrixImpl(result.diagonal(), stdDevs...);
+  return result;
+}
+
+/**
+ * Creates a cost matrix from the given vector for use with LQR.
+ *
+ * The cost matrix is constructed using Bryson's rule. The inverse square of
+ * each element in the input is taken and placed on the cost matrix diagonal.
+ *
+ * @param costs An array. For a Q matrix, its elements are the maximum allowed
+ *              excursions of the states from the reference. For an R matrix,
+ *              its elements are the maximum allowed excursions of the control
+ *              inputs from no actuation.
+ * @return State excursion or control effort cost matrix.
+ */
+template <size_t N>
+Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
+  Eigen::DiagonalMatrix<double, N> result;
+  auto& diag = result.diagonal();
+  for (size_t i = 0; i < N; ++i) {
+    diag(i) = 1.0 / std::pow(costs[i], 2);
+  }
+  return result;
+}
+
+/**
+ * Creates a covariance matrix from the given vector for use with Kalman
+ * filters.
+ *
+ * Each element is squared and placed on the covariance matrix diagonal.
+ *
+ * @param stdDevs An array. For a Q matrix, its elements are the standard
+ *                deviations of each state from how the model behaves. For an R
+ *                matrix, its elements are the standard deviations for each
+ *                output measurement.
+ * @return Process noise or measurement noise covariance matrix.
+ */
+template <size_t N>
+Eigen::Matrix<double, N, N> MakeCovMatrix(
+    const std::array<double, N>& stdDevs) {
+  Eigen::DiagonalMatrix<double, N> result;
+  auto& diag = result.diagonal();
+  for (size_t i = 0; i < N; ++i) {
+    diag(i) = std::pow(stdDevs[i], 2);
+  }
+  return result;
+}
+
+template <typename... Ts, typename = std::enable_if_t<
+                              std::conjunction_v<std::is_same<double, Ts>...>>>
+Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
+  Eigen::Matrix<double, sizeof...(Ts), 1> result;
+  detail::WhiteNoiseVectorImpl(result, stdDevs...);
+  return result;
+}
+
+/**
+ * Creates a vector of normally distributed white noise with the given noise
+ * intensities for each element.
+ *
+ * @param stdDevs An array whose elements are the standard deviations of each
+ *                element of the noise vector.
+ * @return White noise vector.
+ */
+template <int N>
+Eigen::Matrix<double, N, 1> MakeWhiteNoiseVector(
+    const std::array<double, N>& stdDevs) {
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+
+  Eigen::Matrix<double, N, 1> result;
+  for (int i = 0; i < N; ++i) {
+    // Passing a standard deviation of 0.0 to std::normal_distribution is
+    // undefined behavior
+    if (stdDevs[i] == 0.0) {
+      result(i) = 0.0;
+    } else {
+      std::normal_distribution distr{0.0, stdDevs[i]};
+      result(i) = distr(gen);
+    }
+  }
+  return result;
+}
+
+/**
+ * Returns true if (A, B) is a stabilizable pair.
+ *
+ * (A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+ * any, have absolute values less than one, where an eigenvalue is
+ * uncontrollable if rank(lambda * I - A, B) < n where n is number of states.
+ *
+ * @param A System matrix.
+ * @param B Input matrix.
+ */
+template <int States, int Inputs>
+bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
+                    const Eigen::Matrix<double, States, Inputs>& B) {
+  return detail::IsStabilizableImpl<States, Inputs>(A, B);
+}
+
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
+                          const Eigen::Matrix<double, 1, 1>& B);
+
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
+                          const Eigen::Matrix<double, 2, 1>& B);
+
+/**
+ * Converts a Pose2d into a vector of [x, y, theta].
+ *
+ * @param pose The pose that is being represented.
+ *
+ * @return The vector.
+ */
+Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose);
+
+/**
+ * Clamps input vector between system's minimum and maximum allowable input.
+ *
+ * @param u Input vector to clamp.
+ * @return Clamped input vector.
+ */
+template <int Inputs>
+Eigen::Matrix<double, Inputs, 1> ClampInputMaxMagnitude(
+    const Eigen::Matrix<double, Inputs, 1>& u,
+    const Eigen::Matrix<double, Inputs, 1>& umin,
+    const Eigen::Matrix<double, Inputs, 1>& umax) {
+  Eigen::Matrix<double, Inputs, 1> result;
+  for (int i = 0; i < Inputs; ++i) {
+    result(i) = std::clamp(u(i), umin(i), umax(i));
+  }
+  return result;
+}
+
+/**
+ * Normalize all inputs if any excedes the maximum magnitude. Useful for systems
+ * such as differential drivetrains.
+ *
+ * @param u            The input vector.
+ * @param maxMagnitude The maximum magnitude any input can have.
+ * @param <I>          The number of inputs.
+ * @return The normalizedInput
+ */
+template <int Inputs>
+Eigen::Matrix<double, Inputs, 1> NormalizeInputVector(
+    const Eigen::Matrix<double, Inputs, 1>& u, double maxMagnitude) {
+  double maxValue = u.template lpNorm<Eigen::Infinity>();
+
+  if (maxValue > maxMagnitude) {
+    return u * maxMagnitude / maxValue;
+  }
+  return u;
+}
+}  // namespace frc

wpimath/src/main/native/include/frc/controller/ArmFeedforward.h

diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
new file mode 100644
index 0000000..14df187
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -0,0 +1,153 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+
+#include "units/angle.h"
+#include "units/angular_velocity.h"
+#include "units/math.h"
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple arm (modeled as
+ * a motor acting against the force of gravity on a beam suspended at an angle).
+ */
+class ArmFeedforward {
+ public:
+  using Angle = units::radians;
+  using Velocity = units::radians_per_second;
+  using Acceleration = units::compound_unit<units::radians_per_second,
+                                            units::inverse<units::second>>;
+  using kv_unit =
+      units::compound_unit<units::volts,
+                           units::inverse<units::radians_per_second>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+  constexpr ArmFeedforward() = default;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains.
+   *
+   * @param kS   The static gain, in volts.
+   * @param kCos 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::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+      : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param angle        The angle setpoint, in radians.
+   * @param velocity     The velocity setpoint, in radians per second.
+   * @param acceleration The acceleration setpoint, in radians per second^2.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Angle> angle,
+                          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) +
+           kV * velocity + kA * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param acceleration The acceleration of the arm.
+   * @return The maximum possible velocity at the given acceleration and angle.
+   */
+  units::unit_t<Velocity> MaxAchievableVelocity(
+      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) -
+            kA * acceleration) /
+           kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param acceleration The acceleration of the arm.
+   * @return The minimum possible velocity at the given acceleration and angle.
+   */
+  units::unit_t<Velocity> MinAchievableVelocity(
+      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) -
+            kA * acceleration) /
+           kV;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply, a position, 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param velocity The velocity of the arm.
+   * @return The maximum possible acceleration at the given velocity and angle.
+   */
+  units::unit_t<Acceleration> MaxAchievableAcceleration(
+      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) /
+           kA;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply, a position, 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param velocity The velocity of the arm.
+   * @return The minimum possible acceleration at the given velocity and angle.
+   */
+  units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Velocity> velocity) {
+    return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+
+  units::volt_t kS{0};
+  units::volt_t kCos{0};
+  units::unit_t<kv_unit> kV{0};
+  units::unit_t<ka_unit> kA{0};
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h

diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
new file mode 100644
index 0000000..134ed97
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -0,0 +1,203 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <functional>
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/system/NumericalJacobian.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Constructs a control-affine plant inversion model-based feedforward from
+ * given model dynamics.
+ *
+ * If given the vector valued function as f(x, u) where x is the state
+ * vector and u is the input vector, the B matrix(continuous input matrix)
+ * is calculated through a NumericalJacobian. In this case f has to be
+ * control-affine (of the form f(x) + Bu).
+ *
+ * The feedforward is calculated as
+ * <strong> u_ff = B<sup>+</sup> (rDot - f(x)) </strong>, where <strong>
+ * B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * This feedforward does not account for a dynamic B matrix, B is either
+ * determined or supplied when the feedforward is created and remains constant.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+template <int States, int Inputs>
+class ControlAffinePlantInversionFeedforward {
+ public:
+  /**
+   * Constructs a feedforward with given model dynamics as a function
+   * of state and input.
+   *
+   * @param f  A vector-valued function of x, the state, and
+   *           u, the input, that returns the derivative of
+   *           the state vector. HAS to be control-affine
+   *           (of the form f(x) + Bu).
+   * @param dt The timestep between calls of calculate().
+   */
+  ControlAffinePlantInversionFeedforward(
+      std::function<Eigen::Matrix<double, States, 1>(
+          const Eigen::Matrix<double, States, 1>&,
+          const Eigen::Matrix<double, Inputs, 1>&)>
+          f,
+      units::second_t dt)
+      : m_dt(dt), m_f(f) {
+    m_B = NumericalJacobianU<States, States, Inputs>(
+        f, Eigen::Matrix<double, States, 1>::Zero(),
+        Eigen::Matrix<double, Inputs, 1>::Zero());
+
+    m_r.setZero();
+    Reset(m_r);
+  }
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function of state,
+   * and the plant's B matrix(continuous input matrix).
+   *
+   * @param f  A vector-valued function of x, the state,
+   *           that returns the derivative of the state vector.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param dt The timestep between calls of calculate().
+   */
+  ControlAffinePlantInversionFeedforward(
+      std::function<Eigen::Matrix<double, States, 1>(
+          const Eigen::Matrix<double, States, 1>&)>
+          f,
+      const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
+      : m_B(B), m_dt(dt) {
+    m_f = [=](const Eigen::Matrix<double, States, 1>& x,
+              const Eigen::Matrix<double, Inputs, 1>& u)
+        -> Eigen::Matrix<double, States, 1> { return f(x); };
+
+    m_r.setZero();
+    Reset(m_r);
+  }
+
+  ControlAffinePlantInversionFeedforward(
+      ControlAffinePlantInversionFeedforward&&) = default;
+  ControlAffinePlantInversionFeedforward& operator=(
+      ControlAffinePlantInversionFeedforward&&) = default;
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   *
+   * @return The row of the calculated feedforward.
+   */
+  double Uff(int i) const { return m_uff(i, 0); }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param i Row of r.
+   *
+   * @return The row of the current reference vector.
+   */
+  double R(int i) const { return m_r(i, 0); }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
+    m_r = initialState;
+    m_uff.setZero();
+  }
+
+  /**
+   * Resets the feedforward with a zero initial state vector.
+   */
+  void Reset() {
+    m_r.setZero();
+    m_uff.setZero();
+  }
+
+  /**
+   * Calculate the feedforward with only the desired
+   * future reference. This uses the internally stored "current"
+   * reference.
+   *
+   * If this method is used the initial state of the system is the one
+   * set using Reset(const Eigen::Matrix<double, States, 1>&).
+   * If the initial state is not set it defaults to a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  Eigen::Matrix<double, Inputs, 1> Calculate(
+      const Eigen::Matrix<double, States, 1>& nextR) {
+    return Calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r     The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  Eigen::Matrix<double, Inputs, 1> Calculate(
+      const Eigen::Matrix<double, States, 1>& r,
+      const Eigen::Matrix<double, States, 1>& nextR) {
+    Eigen::Matrix<double, States, 1> rDot = (nextR - r) / m_dt.to<double>();
+
+    m_uff = m_B.householderQr().solve(
+        rDot - m_f(r, Eigen::Matrix<double, Inputs, 1>::Zero()));
+
+    m_r = nextR;
+    return m_uff;
+  }
+
+ private:
+  Eigen::Matrix<double, States, Inputs> m_B;
+
+  units::second_t m_dt;
+
+  /**
+   * The model dynamics.
+   */
+  std::function<Eigen::Matrix<double, States, 1>(
+      const Eigen::Matrix<double, States, 1>&,
+      const Eigen::Matrix<double, Inputs, 1>&)>
+      m_f;
+
+  // Current reference
+  Eigen::Matrix<double, States, 1> m_r;
+
+  // Computed feedforward
+  Eigen::Matrix<double, Inputs, 1> m_uff;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h

diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
new file mode 100644
index 0000000..b82d960
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple elevator
+ * (modeled as a motor acting against the force of gravity).
+ */
+template <class Distance>
+class ElevatorFeedforward {
+ public:
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+  ElevatorFeedforward() = default;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains.
+   *
+   * @param kS The static gain, in volts.
+   * @param kG The gravity gain, in volts.
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   */
+  constexpr ElevatorFeedforward(
+      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), kG(kG), kV(kV), kA(kA) {}
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint, in distance per second.
+   * @param acceleration The acceleration setpoint, in distance per second^2.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+                                    units::unit_t<Acceleration> acceleration =
+                                        units::unit_t<Acceleration>(0)) {
+    return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kG - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + kS - kG - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the maximum 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
+  }
+
+  /**
+   * 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return MaxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  units::volt_t kS{0};
+  units::volt_t kG{0};
+  units::unit_t<kv_unit> kV{0};
+  units::unit_t<ka_unit> kA{0};
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h

diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
new file mode 100644
index 0000000..ea86d90
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <functional>
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/system/Discretization.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Constructs a plant inversion model-based feedforward from a LinearSystem.
+ *
+ * The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A
+ * r_k) </strong>, where <strong> B<sup>+</sup> </strong> is the pseudoinverse
+ * of B.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+template <int States, int Inputs>
+class LinearPlantInversionFeedforward {
+ public:
+  /**
+   * Constructs a feedforward with the given plant.
+   *
+   * @param plant     The plant being controlled.
+   * @param dtSeconds Discretization timestep.
+   */
+  template <int Outputs>
+  LinearPlantInversionFeedforward(
+      const LinearSystem<States, Inputs, Outputs>& plant, units::second_t dt)
+      : LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}
+
+  /**
+   * Constructs a feedforward with the given coefficients.
+   *
+   * @param A         Continuous system matrix of the plant being controlled.
+   * @param B         Continuous input matrix of the plant being controlled.
+   * @param dtSeconds Discretization timestep.
+   */
+  LinearPlantInversionFeedforward(
+      const Eigen::Matrix<double, States, States>& A,
+      const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
+      : m_dt(dt) {
+    DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
+
+    m_r.setZero();
+    Reset(m_r);
+  }
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   *
+   * @return The row of the calculated feedforward.
+   */
+  double Uff(int i) const { return m_uff(i, 0); }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param i Row of r.
+   *
+   * @return The row of the current reference vector.
+   */
+  double R(int i) const { return m_r(i, 0); }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
+    m_r = initialState;
+    m_uff.setZero();
+  }
+
+  /**
+   * Resets the feedforward with a zero initial state vector.
+   */
+  void Reset() {
+    m_r.setZero();
+    m_uff.setZero();
+  }
+
+  /**
+   * Calculate the feedforward with only the desired
+   * future reference. This uses the internally stored "current"
+   * reference.
+   *
+   * If this method is used the initial state of the system is the one
+   * set using Reset(const Eigen::Matrix<double, States, 1>&).
+   * If the initial state is not set it defaults to a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  Eigen::Matrix<double, Inputs, 1> Calculate(
+      const Eigen::Matrix<double, States, 1>& nextR) {
+    return Calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r     The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   *
+   * @return The calculated feedforward.
+   */
+  Eigen::Matrix<double, Inputs, 1> Calculate(
+      const Eigen::Matrix<double, States, 1>& r,
+      const Eigen::Matrix<double, States, 1>& nextR) {
+    m_uff = m_B.householderQr().solve(nextR - (m_A * r));
+    m_r = nextR;
+    return m_uff;
+  }
+
+ private:
+  Eigen::Matrix<double, States, States> m_A;
+  Eigen::Matrix<double, States, Inputs> m_B;
+
+  units::second_t m_dt;
+
+  // Current reference
+  Eigen::Matrix<double, States, 1> m_r;
+
+  // Computed feedforward
+  Eigen::Matrix<double, Inputs, 1> m_uff;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h

diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
new file mode 100644
index 0000000..f448957
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -0,0 +1,302 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include "Eigen/Core"
+#include "Eigen/src/Cholesky/LLT.h"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+
+namespace frc {
+namespace detail {
+
+/**
+ * Contains the controller coefficients and logic for a linear-quadratic
+ * regulator (LQR).
+ * LQRs use the control law u = K(r - x).
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+template <int States, int Inputs>
+class LinearQuadraticRegulatorImpl {
+ public:
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param plant  The plant being controlled.
+   * @param Qelems The maximum desired error tolerance for each state.
+   * @param Relems The maximum desired control effort for each input.
+   * @param dt     Discretization timestep.
+   */
+  template <int Outputs>
+  LinearQuadraticRegulatorImpl(
+      const LinearSystem<States, Inputs, Outputs>& plant,
+      const std::array<double, States>& Qelems,
+      const std::array<double, Inputs>& Relems, units::second_t dt)
+      : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A      Continuous system matrix of the plant being controlled.
+   * @param B      Continuous input matrix of the plant being controlled.
+   * @param Qelems The maximum desired error tolerance for each state.
+   * @param Relems The maximum desired control effort for each input.
+   * @param dt     Discretization timestep.
+   */
+  LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
+                               const Eigen::Matrix<double, States, Inputs>& B,
+                               const std::array<double, States>& Qelems,
+                               const std::array<double, Inputs>& Relems,
+                               units::second_t dt)
+      : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
+                                     MakeCostMatrix(Relems), dt) {}
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A      Continuous system matrix of the plant being controlled.
+   * @param B      Continuous input matrix of the plant being controlled.
+   * @param Q      The state cost matrix.
+   * @param R      The input cost matrix.
+   * @param dt     Discretization timestep.
+   */
+  LinearQuadraticRegulatorImpl(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,
+                               units::second_t dt) {
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+    Eigen::Matrix<double, States, States> S =
+        drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+    Eigen::Matrix<double, Inputs, Inputs> tmp =
+        discB.transpose() * S * discB + R;
+    m_K = tmp.llt().solve(discB.transpose() * S * discA);
+
+    Reset();
+  }
+
+  LinearQuadraticRegulatorImpl(LinearQuadraticRegulatorImpl&&) = default;
+  LinearQuadraticRegulatorImpl& operator=(LinearQuadraticRegulatorImpl&&) =
+      default;
+
+  /**
+   * Returns the controller matrix K.
+   */
+  const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
+
+  /**
+   * Returns an element of the controller matrix K.
+   *
+   * @param i Row of K.
+   * @param j Column of K.
+   */
+  double K(int i, int j) const { return m_K(i, j); }
+
+  /**
+   * Returns the reference vector r.
+   *
+   * @return The reference vector.
+   */
+  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param i Row of r.
+   *
+   * @return The row of the reference vector.
+   */
+  double R(int i) const { return m_r(i, 0); }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  const Eigen::Matrix<double, Inputs, 1>& U() const { return m_u; }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param i Row of u.
+   *
+   * @return The row of the control input vector.
+   */
+  double U(int i) const { return m_u(i, 0); }
+
+  /**
+   * Resets the controller.
+   */
+  void Reset() {
+    m_r.setZero();
+    m_u.setZero();
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   */
+  Eigen::Matrix<double, Inputs, 1> Calculate(
+      const Eigen::Matrix<double, States, 1>& x) {
+    m_u = m_K * (m_r - x);
+    return m_u;
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x     The current state x.
+   * @param nextR The next reference vector r.
+   */
+  Eigen::Matrix<double, Inputs, 1> Calculate(
+      const Eigen::Matrix<double, States, 1>& x,
+      const Eigen::Matrix<double, States, 1>& nextR) {
+    m_r = nextR;
+    return Calculate(x);
+  }
+
+ private:
+  // Current reference
+  Eigen::Matrix<double, States, 1> m_r;
+
+  // Computed controller output
+  Eigen::Matrix<double, Inputs, 1> m_u;
+
+  // Controller gain
+  Eigen::Matrix<double, Inputs, States> m_K;
+};
+
+}  // namespace detail
+
+template <int States, int Inputs>
+class LinearQuadraticRegulator
+    : public detail::LinearQuadraticRegulatorImpl<States, Inputs> {
+ public:
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param system The plant being controlled.
+   * @param Qelems The maximum desired error tolerance for each state.
+   * @param Relems The maximum desired control effort for each input.
+   * @param dt     Discretization timestep.
+   */
+  template <int Outputs>
+  LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
+                           const std::array<double, States>& Qelems,
+                           const std::array<double, Inputs>& Relems,
+                           units::second_t dt)
+      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A      Continuous system matrix of the plant being controlled.
+   * @param B      Continuous input matrix of the plant being controlled.
+   * @param Qelems The maximum desired error tolerance for each state.
+   * @param Relems The maximum desired control effort for each input.
+   * @param dt     Discretization timestep.
+   */
+  LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
+                           const Eigen::Matrix<double, States, Inputs>& B,
+                           const std::array<double, States>& Qelems,
+                           const std::array<double, Inputs>& Relems,
+                           units::second_t dt)
+      : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                                 MakeCostMatrix(Relems), dt) {}
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A      Continuous system matrix of the plant being controlled.
+   * @param B      Continuous input matrix of the plant being controlled.
+   * @param Q      The state cost matrix.
+   * @param R      The input cost matrix.
+   * @param dt     Discretization timestep.
+   */
+  LinearQuadraticRegulator(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,
+                           units::second_t dt)
+      : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
+
+  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
+};
+
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+class LinearQuadraticRegulator<1, 1>
+    : public detail::LinearQuadraticRegulatorImpl<1, 1> {
+ public:
+  template <int Outputs>
+  LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
+                           const std::array<double, 1>& Qelems,
+                           const std::array<double, 1>& Relems,
+                           units::second_t dt)
+      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
+                           const Eigen::Matrix<double, 1, 1>& B,
+                           const std::array<double, 1>& Qelems,
+                           const std::array<double, 1>& Relems,
+                           units::second_t dt);
+
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
+                           const Eigen::Matrix<double, 1, 1>& B,
+                           const Eigen::Matrix<double, 1, 1>& Q,
+                           const Eigen::Matrix<double, 1, 1>& R,
+                           units::second_t dt);
+
+  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
+};
+
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+class LinearQuadraticRegulator<2, 1>
+    : public detail::LinearQuadraticRegulatorImpl<2, 1> {
+ public:
+  template <int Outputs>
+  LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
+                           const std::array<double, 2>& Qelems,
+                           const std::array<double, 1>& Relems,
+                           units::second_t dt)
+      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
+                           const Eigen::Matrix<double, 2, 1>& B,
+                           const std::array<double, 2>& Qelems,
+                           const std::array<double, 1>& Relems,
+                           units::second_t dt);
+
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
+                           const Eigen::Matrix<double, 2, 1>& B,
+                           const Eigen::Matrix<double, 2, 2>& Q,
+                           const Eigen::Matrix<double, 1, 1>& R,
+                           units::second_t dt);
+
+  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h

diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
new file mode 100644
index 0000000..1afab40
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+
+#include "units/time.h"
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A helper class that computes feedforward voltages for a simple
+ * permanent-magnet DC motor.
+ */
+template <class Distance>
+class SimpleMotorFeedforward {
+ public:
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+  constexpr SimpleMotorFeedforward() = default;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains.
+   *
+   * @param kS The static gain, in volts.
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   */
+  constexpr SimpleMotorFeedforward(
+      units::volt_t kS, units::unit_t<kv_unit> kV,
+      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+      : kS(kS), kV(kV), kA(kA) {}
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint, in distance per second.
+   * @param acceleration The acceleration setpoint, in distance per second^2.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+                                    units::unit_t<Acceleration> acceleration =
+                                        units::unit_t<Acceleration>(0)) const {
+    return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage,
+      units::unit_t<Acceleration> acceleration) const {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage,
+      units::unit_t<Acceleration> acceleration) const {
+    // Assume min velocity is positive, ks flips sign
+    return (-maxVoltage + kS - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the maximum 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
+    return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
+  }
+
+  /**
+   * 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.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
+    return MaxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  units::volt_t kS{0};
+  units::unit_t<kv_unit> kV{0};
+  units::unit_t<ka_unit> kA{0};
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
new file mode 100644
index 0000000..6f0fc85
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -0,0 +1,234 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <functional>
+
+#include "Eigen/Core"
+#include "Eigen/src/Cholesky/LDLT.h"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalJacobian.h"
+#include "frc/system/RungeKutta.h"
+#include "units/time.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+class ExtendedKalmanFilter {
+ public:
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   */
+  ExtendedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
+                           const Eigen::Matrix<double, States, 1>&,
+                           const Eigen::Matrix<double, Inputs, 1>&)>
+                           f,
+                       std::function<Eigen::Matrix<double, Outputs, 1>(
+                           const Eigen::Matrix<double, States, 1>&,
+                           const Eigen::Matrix<double, Inputs, 1>&)>
+                           h,
+                       const std::array<double, States>& stateStdDevs,
+                       const std::array<double, Outputs>& measurementStdDevs,
+                       units::second_t dt)
+      : m_f(f), m_h(h) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_dt = dt;
+
+    Reset();
+
+    Eigen::Matrix<double, States, States> contA =
+        NumericalJacobianX<States, States, Inputs>(
+            m_f, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+    Eigen::Matrix<double, Outputs, States> C =
+        NumericalJacobianX<Outputs, States, Inputs>(
+            m_h, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, States> discQ;
+    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+    Eigen::Matrix<double, Outputs, Outputs> discR =
+        DiscretizeR<Outputs>(m_contR, dt);
+
+    // IsStabilizable(A^T, C^T) will tell us if the system is observable.
+    bool isObservable =
+        IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
+    if (isObservable && Outputs <= States) {
+      m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
+          discA.transpose(), C.transpose(), discQ, discR);
+    } else {
+      m_initP = Eigen::Matrix<double, States, States>::Zero();
+    }
+    m_P = m_initP;
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   */
+  const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param i Row of P.
+   * @param j Column of P.
+   */
+  double P(int i, int j) const { return m_P(i, j); }
+
+  /**
+   * Set the current error covariance matrix P.
+   *
+   * @param P The error covariance matrix P.
+   */
+  void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i, 0); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() {
+    m_xHat.setZero();
+    m_P = m_initP;
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+    m_dt = dt;
+
+    // Find continuous A
+    Eigen::Matrix<double, States, States> contA =
+        NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+
+    // Find discrete A and Q
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, States> discQ;
+    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+    m_xHat = RungeKutta(m_f, m_xHat, u, dt);
+    m_P = discA * m_P * discA.transpose() + discQ;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
+               const Eigen::Matrix<double, Outputs, 1>& y) {
+    Correct<Outputs>(u, y, m_h, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns
+   *          the measurement vector.
+   * @param R Discrete measurement noise covariance matrix.
+   */
+  template <int Rows>
+  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
+               const Eigen::Matrix<double, Rows, 1>& y,
+               std::function<Eigen::Matrix<double, Rows, 1>(
+                   const Eigen::Matrix<double, States, 1>&,
+                   const Eigen::Matrix<double, Inputs, 1>&)>
+                   h,
+               const Eigen::Matrix<double, Rows, Rows>& R) {
+    const Eigen::Matrix<double, Rows, States> C =
+        NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
+    const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+
+    Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
+
+    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PC^T S^-1
+    // KS = PC^T
+    // (KS)^T = (PC^T)^T
+    // S^T K^T = CP^T
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // K^T = S^T.solve(CP^T)
+    // K = (S^T.solve(CP^T))^T
+    Eigen::Matrix<double, States, Rows> K =
+        S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
+
+    m_xHat += K * (y - h(m_xHat, u));
+    m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
+  }
+
+ private:
+  std::function<Eigen::Matrix<double, States, 1>(
+      const Eigen::Matrix<double, States, 1>&,
+      const Eigen::Matrix<double, Inputs, 1>&)>
+      m_f;
+  std::function<Eigen::Matrix<double, Outputs, 1>(
+      const Eigen::Matrix<double, States, 1>&,
+      const Eigen::Matrix<double, Inputs, 1>&)>
+      m_h;
+  Eigen::Matrix<double, States, 1> m_xHat;
+  Eigen::Matrix<double, States, States> m_P;
+  Eigen::Matrix<double, States, States> m_contQ;
+  Eigen::Matrix<double, Outputs, Outputs> m_contR;
+  units::second_t m_dt;
+
+  Eigen::Matrix<double, States, States> m_initP;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/KalmanFilter.h

diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
new file mode 100644
index 0000000..c395080
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -0,0 +1,237 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <cmath>
+
+#include "Eigen/Core"
+#include "Eigen/src/Cholesky/LDLT.h"
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+namespace detail {
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ */
+template <int States, int Inputs, int Outputs>
+class KalmanFilterImpl {
+ public:
+  /**
+   * Constructs a state-space observer with the given plant.
+   *
+   * @param plant              The plant used for the prediction step.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   */
+  KalmanFilterImpl(LinearSystem<States, Inputs, Outputs>& plant,
+                   const std::array<double, States>& stateStdDevs,
+                   const std::array<double, Outputs>& measurementStdDevs,
+                   units::second_t dt) {
+    m_plant = &plant;
+
+    auto contQ = MakeCovMatrix(stateStdDevs);
+    auto contR = MakeCovMatrix(measurementStdDevs);
+
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, States> discQ;
+    DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
+
+    auto discR = DiscretizeR<Outputs>(contR, dt);
+
+    const auto& C = plant.C();
+
+    // IsStabilizable(A^T, C^T) will tell us if the system is observable.
+    bool isObservable =
+        IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
+    if (!isObservable) {
+      wpi::math::MathSharedStore::ReportError(
+          "The system passed to the Kalman filter is not observable!");
+      throw std::invalid_argument(
+          "The system passed to the Kalman filter is not observable!");
+    }
+
+    Eigen::Matrix<double, States, States> P =
+        drake::math::DiscreteAlgebraicRiccatiEquation(
+            discA.transpose(), C.transpose(), discQ, discR);
+
+    Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
+
+    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PC^T S^-1
+    // KS = PC^T
+    // (KS)^T = (PC^T)^T
+    // S^T K^T = CP^T
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // K^T = S^T.solve(CP^T)
+    // K = (S^T.solve(CP^T))^T
+    m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
+
+    Reset();
+  }
+
+  KalmanFilterImpl(KalmanFilterImpl&&) = default;
+  KalmanFilterImpl& operator=(KalmanFilterImpl&&) = default;
+
+  /**
+   * Returns the steady-state Kalman gain matrix K.
+   */
+  const Eigen::Matrix<double, States, Outputs>& K() const { return m_K; }
+
+  /**
+   * Returns an element of the steady-state Kalman gain matrix K.
+   *
+   * @param i Row of K.
+   * @param j Column of K.
+   */
+  double K(int i, int j) const { return m_K(i, j); }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() { m_xHat.setZero(); }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+    m_xHat = m_plant->CalculateX(m_xHat, u, dt);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the last predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
+               const Eigen::Matrix<double, Outputs, 1>& y) {
+    m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
+  }
+
+ private:
+  LinearSystem<States, Inputs, Outputs>* m_plant;
+
+  /**
+   * The steady-state Kalman gain matrix.
+   */
+  Eigen::Matrix<double, States, Outputs> m_K;
+
+  /**
+   * The state estimate.
+   */
+  Eigen::Matrix<double, States, 1> m_xHat;
+};
+
+}  // namespace detail
+
+template <int States, int Inputs, int Outputs>
+class KalmanFilter : public detail::KalmanFilterImpl<States, Inputs, Outputs> {
+ public:
+  /**
+   * Constructs a state-space observer with the given plant.
+   *
+   * @param plant              The plant used for the prediction step.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   */
+  KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
+               const std::array<double, States>& stateStdDevs,
+               const std::array<double, Outputs>& measurementStdDevs,
+               units::second_t dt)
+      : detail::KalmanFilterImpl<States, Inputs, Outputs>{
+            plant, stateStdDevs, measurementStdDevs, dt} {}
+
+  KalmanFilter(KalmanFilter&&) = default;
+  KalmanFilter& operator=(KalmanFilter&&) = default;
+};
+
+// Template specializations are used here to make common state-input-output
+// triplets compile faster.
+template <>
+class KalmanFilter<1, 1, 1> : public detail::KalmanFilterImpl<1, 1, 1> {
+ public:
+  KalmanFilter(LinearSystem<1, 1, 1>& plant,
+               const std::array<double, 1>& stateStdDevs,
+               const std::array<double, 1>& measurementStdDevs,
+               units::second_t dt);
+
+  KalmanFilter(KalmanFilter&&) = default;
+  KalmanFilter& operator=(KalmanFilter&&) = default;
+};
+
+// Template specializations are used here to make common state-input-output
+// triplets compile faster.
+template <>
+class KalmanFilter<2, 1, 1> : public detail::KalmanFilterImpl<2, 1, 1> {
+ public:
+  KalmanFilter(LinearSystem<2, 1, 1>& plant,
+               const std::array<double, 2>& stateStdDevs,
+               const std::array<double, 1>& measurementStdDevs,
+               units::second_t dt);
+
+  KalmanFilter(KalmanFilter&&) = default;
+  KalmanFilter& operator=(KalmanFilter&&) = default;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h

diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
new file mode 100644
index 0000000..72abb80
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "Eigen/src/Cholesky/LLT.h"
+
+namespace frc {
+
+/**
+ * Generates sigma points and weights according to Van der Merwe's 2004
+ * dissertation[1] for the UnscentedKalmanFilter class.
+ *
+ * It parametrizes the sigma points using alpha, beta, kappa terms, and is the
+ * version seen in most publications. Unless you know better, this should be
+ * your default choice.
+ *
+ * @tparam States The dimensionality of the state. 2*States+1 weights will be
+ *                generated.
+ *
+ * [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
+ *     Inference in Dynamic State-Space Models" (Doctoral dissertation)
+ */
+template <int States>
+class MerweScaledSigmaPoints {
+ public:
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points.
+   *
+   * @param alpha Determines the spread of the sigma points around the mean.
+   *              Usually a small positive value (1e-3).
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   *             For Gaussian distributions, beta = 2 is optimal.
+   * @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
+   */
+  MerweScaledSigmaPoints(double alpha = 1e-3, double beta = 2,
+                         int kappa = 3 - States) {
+    m_alpha = alpha;
+    m_kappa = kappa;
+
+    ComputeWeights(beta);
+  }
+
+  /**
+   * Returns number of sigma points for each variable in the state x.
+   */
+  int NumSigmas() { return 2 * States + 1; }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean
+   * (x) and covariance(P) of the filter.
+   *
+   * @param x An array of the means.
+   * @param P Covariance of the filter.
+   *
+   * @return Two dimensional array of sigma points. Each column contains all of
+   *         the sigmas for one dimension in the problem space. Ordered by
+   *         Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
+   *
+   */
+  Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
+      const Eigen::Matrix<double, States, 1>& x,
+      const Eigen::Matrix<double, States, States>& P) {
+    double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
+    Eigen::Matrix<double, States, States> U =
+        ((lambda + States) * P).llt().matrixL();
+
+    Eigen::Matrix<double, States, 2 * States + 1> sigmas;
+    sigmas.template block<States, 1>(0, 0) = x;
+    for (int k = 0; k < States; ++k) {
+      sigmas.template block<States, 1>(0, k + 1) =
+          x + U.template block<States, 1>(0, k);
+      sigmas.template block<States, 1>(0, States + k + 1) =
+          x - U.template block<States, 1>(0, k);
+    }
+
+    return sigmas;
+  }
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   */
+  const Eigen::Matrix<double, 2 * States + 1, 1>& Wm() const { return m_Wm; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the mean.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wm(int i) const { return m_Wm(i, 0); }
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   */
+  const Eigen::Matrix<double, 2 * States + 1, 1>& Wc() const { return m_Wc; }
+
+  /**
+   * Returns an element of the weight for each sigma point for the covariance.
+   *
+   * @param i Element of vector to return.
+   */
+  double Wc(int i) const { return m_Wc(i, 0); }
+
+ private:
+  Eigen::Matrix<double, 2 * States + 1, 1> m_Wm;
+  Eigen::Matrix<double, 2 * States + 1, 1> m_Wc;
+  double m_alpha;
+  int m_kappa;
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   */
+  void ComputeWeights(double beta) {
+    double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
+
+    double c = 0.5 / (States + lambda);
+    m_Wm = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
+    m_Wc = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
+
+    m_Wm(0) = lambda / (States + lambda);
+    m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
+  }
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h

diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
new file mode 100644
index 0000000..8c2c31f
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -0,0 +1,235 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <functional>
+
+#include "Eigen/Core"
+#include "Eigen/src/Cholesky/LDLT.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/MerweScaledSigmaPoints.h"
+#include "frc/estimator/UnscentedTransform.h"
+#include "frc/system/Discretization.h"
+#include "frc/system/NumericalJacobian.h"
+#include "frc/system/RungeKutta.h"
+#include "units/time.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs>
+class UnscentedKalmanFilter {
+ public:
+  /**
+   * Constructs an unscented Kalman filter.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dt                 Nominal discretization timestep.
+   */
+  UnscentedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
+                            const Eigen::Matrix<double, States, 1>&,
+                            const Eigen::Matrix<double, Inputs, 1>&)>
+                            f,
+                        std::function<Eigen::Matrix<double, Outputs, 1>(
+                            const Eigen::Matrix<double, States, 1>&,
+                            const Eigen::Matrix<double, Inputs, 1>&)>
+                            h,
+                        const std::array<double, States>& stateStdDevs,
+                        const std::array<double, Outputs>& measurementStdDevs,
+                        units::second_t dt)
+      : m_f(f), m_h(h) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_dt = dt;
+
+    Reset();
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   */
+  const Eigen::Matrix<double, States, States>& P() const { return m_P; }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param i Row of P.
+   * @param j Column of P.
+   */
+  double P(int i, int j) const { return m_P(i, j); }
+
+  /**
+   * Set the current error covariance matrix P.
+   *
+   * @param P The error covariance matrix P.
+   */
+  void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
+
+  /**
+   * Returns the state estimate x-hat.
+   */
+  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_xHat(i, 0); }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_xHat(i, 0) = value; }
+
+  /**
+   * Resets the observer.
+   */
+  void Reset() {
+    m_xHat.setZero();
+    m_P.setZero();
+    m_sigmasF.setZero();
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u  New control input from controller.
+   * @param dt Timestep for prediction.
+   */
+  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+    m_dt = dt;
+
+    // Discretize Q before projecting mean and covariance forward
+    Eigen::Matrix<double, States, States> contA =
+        NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, States> discQ;
+    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+    Eigen::Matrix<double, States, 2 * States + 1> sigmas =
+        m_pts.SigmaPoints(m_xHat, m_P);
+
+    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+      Eigen::Matrix<double, States, 1> x =
+          sigmas.template block<States, 1>(0, i);
+      m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
+    }
+
+    auto ret =
+        UnscentedTransform<States, States>(m_sigmasF, m_pts.Wm(), m_pts.Wc());
+    m_xHat = std::get<0>(ret);
+    m_P = std::get<1>(ret);
+
+    m_P += discQ;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
+               const Eigen::Matrix<double, Outputs, 1>& y) {
+    Correct<Outputs>(u, y, m_h, m_contR);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns
+   *          the measurement vector.
+   * @param R Measurement noise covariance matrix.
+   */
+  template <int Rows>
+  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
+               const Eigen::Matrix<double, Rows, 1>& y,
+               std::function<Eigen::Matrix<double, Rows, 1>(
+                   const Eigen::Matrix<double, States, 1>&,
+                   const Eigen::Matrix<double, Inputs, 1>&)>
+                   h,
+               const Eigen::Matrix<double, Rows, Rows>& R) {
+    const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+
+    // Transform sigma points into measurement space
+    Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
+    Eigen::Matrix<double, States, 2 * States + 1> sigmas =
+        m_pts.SigmaPoints(m_xHat, m_P);
+    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+      sigmasH.template block<Rows, 1>(0, i) =
+          h(sigmas.template block<States, 1>(0, i), u);
+    }
+
+    // Mean and covariance of prediction passed through UT
+    auto [yHat, Py] =
+        UnscentedTransform<States, Rows>(sigmasH, m_pts.Wm(), m_pts.Wc());
+    Py += discR;
+
+    // Compute cross covariance of the state and the measurements
+    Eigen::Matrix<double, States, Rows> Pxy;
+    Pxy.setZero();
+    for (int i = 0; i < m_pts.NumSigmas(); ++i) {
+      Pxy += m_pts.Wc(i) *
+             (m_sigmasF.template block<States, 1>(0, i) - m_xHat) *
+             (sigmasH.template block<Rows, 1>(0, i) - yHat).transpose();
+    }
+
+    // K = P_{xy} Py^-1
+    // K^T = P_y^T^-1 P_{xy}^T
+    // P_y^T K^T = P_{xy}^T
+    // K^T = P_y^T.solve(P_{xy}^T)
+    // K = (P_y^T.solve(P_{xy}^T)^T
+    Eigen::Matrix<double, States, Rows> K =
+        Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
+
+    m_xHat += K * (y - yHat);
+    m_P -= K * Py * K.transpose();
+  }
+
+ private:
+  std::function<Eigen::Matrix<double, States, 1>(
+      const Eigen::Matrix<double, States, 1>&,
+      const Eigen::Matrix<double, Inputs, 1>&)>
+      m_f;
+  std::function<Eigen::Matrix<double, Outputs, 1>(
+      const Eigen::Matrix<double, States, 1>&,
+      const Eigen::Matrix<double, Inputs, 1>&)>
+      m_h;
+  Eigen::Matrix<double, States, 1> m_xHat;
+  Eigen::Matrix<double, States, States> m_P;
+  Eigen::Matrix<double, States, States> m_contQ;
+  Eigen::Matrix<double, Outputs, Outputs> m_contR;
+  Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
+  units::second_t m_dt;
+
+  MerweScaledSigmaPoints<States> m_pts;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h

diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
new file mode 100644
index 0000000..22b32ce
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <tuple>
+
+#include "Eigen/Core"
+
+namespace frc {
+
+/**
+ * Computes unscented transform of a set of sigma points and weights. CovDimurns
+ * the mean and covariance in a tuple.
+ *
+ * This works in conjunction with the UnscentedKalmanFilter class.
+ *
+ * @tparam States  Number of states.
+ * @tparam CovDim  Dimension of covariance of sigma points after passing through
+ *                 the transform.
+ * @param sigmas   List of sigma points.
+ * @param Wm       Weights for the mean.
+ * @param Wc       Weights for the covariance.
+ *
+ * @return Tuple of x, mean of sigma points; P, covariance of sigma points after
+ *         passing through the transform.
+ */
+template <int States, int CovDim>
+std::tuple<Eigen::Matrix<double, CovDim, 1>,
+           Eigen::Matrix<double, CovDim, CovDim>>
+UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
+                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wm,
+                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wc) {
+  // New mean is just the sum of the sigmas * weight
+  // dot = \Sigma^n_1 (W[k]*Xi[k])
+  Eigen::Matrix<double, CovDim, 1> x = sigmas * Wm;
+
+  // New covariance is the sum of the outer product of the residuals times the
+  // weights
+  Eigen::Matrix<double, CovDim, 2 * States + 1> y;
+  for (int i = 0; i < 2 * States + 1; ++i) {
+    y.template block<CovDim, 1>(0, i) =
+        sigmas.template block<CovDim, 1>(0, i) - x;
+  }
+  Eigen::Matrix<double, CovDim, CovDim> P =
+      y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
+
+  return std::make_tuple(x, P);
+}
+
+}  // namespace frc

wpimath/src/main/native/include/frc/geometry/Pose2d.h

diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
new file mode 100644
index 0000000..43f4756
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Transform2d.h"
+#include "Translation2d.h"
+#include "Twist2d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+class Pose2d {
+ public:
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis.
+   * (Translation2d{0, 0} and Rotation{0})
+   */
+  constexpr Pose2d() = default;
+
+  /**
+   * Constructs a pose with the specified translation and rotation.
+   *
+   * @param translation The translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose2d(Translation2d translation, Rotation2d rotation);
+
+  /**
+   * Convenience constructors that takes in x and y values directly instead of
+   * having to construct a Translation2d.
+   *
+   * @param x The x component of the translational component of the pose.
+   * @param y The y component of the translational component of the pose.
+   * @param rotation The rotational component of the pose.
+   */
+  Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Transforms the pose by the given transformation and returns the new
+   * transformed pose.
+   *
+   * [x_new]    [cos, -sin, 0][transform.x]
+   * [y_new] += [sin,  cos, 0][transform.y]
+   * [t_new]    [0,    0,   1][transform.t]
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose2d operator+(const Transform2d& other) const;
+
+  /**
+   * Transforms the current pose by the transformation.
+   *
+   * This is similar to the + operator, except that it mutates the current
+   * object.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return Reference to the new mutated object.
+   */
+  Pose2d& operator+=(const Transform2d& other);
+
+  /**
+   * Returns the Transform2d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  Transform2d operator-(const Pose2d& other) const;
+
+  /**
+   * Checks equality between this Pose2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Pose2d& other) const;
+
+  /**
+   * Checks inequality between this Pose2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Pose2d& other) const;
+
+  /**
+   * Returns the underlying translation.
+   *
+   * @return Reference to the translational component of the pose.
+   */
+  const Translation2d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the underlying rotation.
+   *
+   * @return Reference to the rotational component of the pose.
+   */
+  const Rotation2d& Rotation() const { return m_rotation; }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new pose.
+   * See + operator for the matrix multiplication performed.
+   *
+   * @param other The transform to transform the pose by.
+   *
+   * @return The transformed pose.
+   */
+  Pose2d TransformBy(const Transform2d& other) const;
+
+  /**
+   * Returns the other pose relative to the current pose.
+   *
+   * This function can often be used for trajectory tracking or pose
+   * stabilization algorithms to get the error between the reference and the
+   * current pose.
+   *
+   * @param other The pose that is the origin of the new coordinate frame that
+   * the current pose will be converted into.
+   *
+   * @return The current pose relative to the new origin pose.
+   */
+  Pose2d RelativeTo(const Pose2d& other) const;
+
+  /**
+   * Obtain a new Pose2d from a (constant curvature) velocity.
+   *
+   * See https://file.tavsys.net/control/controls-engineering-in-frc.pdf section
+   * 10.2 "Pose exponential" for a derivation.
+   *
+   * The twist is a change in pose in the robot's coordinate frame since the
+   * previous pose update. When the user runs exp() on the previous known
+   * field-relative pose with the argument being the twist, the user will
+   * receive the new field-relative pose.
+   *
+   * "Exp" represents the pose exponential, which is solving a differential
+   * equation moving the pose forward in time.
+   *
+   * @param twist The change in pose in the robot's coordinate frame since the
+   * previous pose update. For example, if a non-holonomic robot moves forward
+   * 0.01 meters and changes angle by 0.5 degrees since the previous pose
+   * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   *
+   * @return The new pose of the robot.
+   */
+  Pose2d Exp(const Twist2d& twist) const;
+
+  /**
+   * Returns a Twist2d that maps this pose to the end pose. If c is the output
+   * of a.Log(b), then a.Exp(c) would yield b.
+   *
+   * @param end The end pose for the transformation.
+   *
+   * @return The twist that maps this to end.
+   */
+  Twist2d Log(const Pose2d& end) const;
+
+ private:
+  Translation2d m_translation;
+  Rotation2d m_rotation;
+};
+
+void to_json(wpi::json& json, const Pose2d& pose);
+
+void from_json(const wpi::json& json, Pose2d& pose);
+
+}  // namespace frc

wpimath/src/main/native/include/frc/geometry/Rotation2d.h

diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
new file mode 100644
index 0000000..914eba4
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/angle.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ */
+class Rotation2d {
+ public:
+  /**
+   * Constructs a Rotation2d with a default angle of 0 degrees.
+   */
+  constexpr Rotation2d() = default;
+
+  /**
+   * Constructs a Rotation2d with the given radian value.
+   *
+   * @param value The value of the angle in radians.
+   */
+  Rotation2d(units::radian_t value);  // NOLINT(runtime/explicit)
+
+  /**
+   * Constructs a Rotation2d with the given degree value.
+   *
+   * @param value The value of the angle in degrees.
+   */
+  Rotation2d(units::degree_t value);  // NOLINT(runtime/explicit)
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine)
+   * components. The x and y don't have to be normalized.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  Rotation2d(double x, double y);
+
+  /**
+   * Adds two rotations together, with the result being bounded between -pi and
+   * pi.
+   *
+   * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
+   * Rotation2d{-pi/2}
+   *
+   * @param other The rotation to add.
+   *
+   * @return The sum of the two rotations.
+   */
+  Rotation2d operator+(const Rotation2d& other) const;
+
+  /**
+   * Adds a rotation to the current rotation.
+   *
+   * This is similar to the + operator except that it mutates the current
+   * object.
+   *
+   * @param other The rotation to add.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Rotation2d& operator+=(const Rotation2d& other);
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new
+   * rotation.
+   *
+   * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
+   * Rotation2d{-pi/2}
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The difference between the two rotations.
+   */
+  Rotation2d operator-(const Rotation2d& other) const;
+
+  /**
+   * Subtracts the new rotation from the current rotation.
+   *
+   * This is similar to the - operator except that it mutates the current
+   * object.
+   *
+   * @param other The rotation to subtract.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Rotation2d& operator-=(const Rotation2d& other);
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of
+   * the current angular value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  Rotation2d operator-() const;
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation2d.
+   */
+  Rotation2d operator*(double scalar) const;
+
+  /**
+   * Checks equality between this Rotation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Rotation2d& other) const;
+
+  /**
+   * Checks inequality between this Rotation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Rotation2d& other) const;
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   *
+   * value_new = std::atan2(cos_new, sin_new)
+   *
+   * @param other The rotation to rotate by.
+   *
+   * @return The new rotated Rotation2d.
+   */
+  Rotation2d RotateBy(const Rotation2d& other) const;
+
+  /**
+   * Returns the radian value of the rotation.
+   *
+   * @return The radian value of the rotation.
+   */
+  units::radian_t Radians() const { return m_value; }
+
+  /**
+   * Returns the degree value of the rotation.
+   *
+   * @return The degree value of the rotation.
+   */
+  units::degree_t Degrees() const { return m_value; }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  double Cos() const { return m_cos; }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  double Sin() const { return m_sin; }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  double Tan() const { return m_sin / m_cos; }
+
+ private:
+  units::radian_t m_value = 0_rad;
+  double m_cos = 1;
+  double m_sin = 0;
+};
+
+void to_json(wpi::json& json, const Rotation2d& rotation);
+
+void from_json(const wpi::json& json, Rotation2d& rotation);
+
+}  // namespace frc

wpimath/src/main/native/include/frc/geometry/Transform2d.h

diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
new file mode 100644
index 0000000..8f05413
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Translation2d.h"
+
+namespace frc {
+
+class Pose2d;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+class Transform2d {
+ public:
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param final The final pose for the transformation.
+   */
+  Transform2d(Pose2d initial, Pose2d final);
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  Transform2d(Translation2d translation, Rotation2d rotation);
+
+  /**
+   * Constructs the identity transform -- maps an initial pose to itself.
+   */
+  constexpr Transform2d() = default;
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return Reference to the translational component of the transform.
+   */
+  const Translation2d& Translation() const { return m_translation; }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  units::meter_t X() const { return m_translation.X(); }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  units::meter_t Y() const { return m_translation.Y(); }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  const Rotation2d& Rotation() const { return m_rotation; }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  Transform2d Inverse() const;
+
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  Transform2d operator*(double scalar) const {
+    return Transform2d(m_translation * scalar, m_rotation * scalar);
+  }
+
+  /**
+   * Checks equality between this Transform2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Transform2d& other) const;
+
+  /**
+   * Checks inequality between this Transform2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Transform2d& other) const;
+
+ private:
+  Translation2d m_translation;
+  Rotation2d m_rotation;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/geometry/Translation2d.h

diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
new file mode 100644
index 0000000..0c3ee3d
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -0,0 +1,231 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Rotation2d.h"
+#include "units/length.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes.
+ * When the robot is placed on the origin, facing toward the X direction,
+ * moving forward increases the X, whereas moving to the left increases the Y.
+ */
+class Translation2d {
+ public:
+  /**
+   * Constructs a Translation2d with X and Y components equal to zero.
+   */
+  constexpr Translation2d() = default;
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  Translation2d(units::meter_t x, units::meter_t y);
+
+  /**
+   * Constructs a Translation2d with the provided distance and angle. This is
+   * essentially converting from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  Translation2d(units::meter_t distance, const Rotation2d& angle);
+
+  /**
+   * Calculates the distance between two translations in 2d space.
+   *
+   * This function uses the pythagorean theorem to calculate the distance.
+   * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  units::meter_t Distance(const Translation2d& other) const;
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  units::meter_t X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  units::meter_t Y() const { return m_y; }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  units::meter_t Norm() const;
+
+  /**
+   * Applies a rotation to the translation in 2d space.
+   *
+   * This multiplies the translation vector by a counterclockwise rotation
+   * matrix of the given angle.
+   *
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   *
+   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+   * Translation2d of {0, 2}.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  Translation2d RotateBy(const Rotation2d& other) const;
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to
+   * vector addition.
+   *
+   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+   * Translation2d{3.0, 8.0}
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  Translation2d operator+(const Translation2d& other) const;
+
+  /**
+   * Adds the new translation to the current translation.
+   *
+   * This is similar to the + operator, except that the current object is
+   * mutated.
+   *
+   * @param other The translation to add.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator+=(const Translation2d& other);
+
+  /**
+   * Subtracts the other translation from the other translation and returns the
+   * difference.
+   *
+   * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+   * Translation2d{4.0, 2.0}
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  Translation2d operator-(const Translation2d& other) const;
+
+  /**
+   * Subtracts the new translation from the current translation.
+   *
+   * This is similar to the - operator, except that the current object is
+   * mutated.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator-=(const Translation2d& other);
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * rotating by 180 degrees, flipping the point over both axes, or simply
+   * negating both components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  Translation2d operator-() const;
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  Translation2d operator*(double scalar) const;
+
+  /**
+   * Multiplies the current translation by a scalar.
+   *
+   * This is similar to the * operator, except that current object is mutated.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator*=(double scalar);
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  Translation2d operator/(double scalar) const;
+
+  /**
+   * Checks equality between this Translation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Translation2d& other) const;
+
+  /**
+   * Checks inequality between this Translation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Translation2d& other) const;
+
+  /*
+   * Divides the current translation by a scalar.
+   *
+   * This is similar to the / operator, except that current object is mutated.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator/=(double scalar);
+
+ private:
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
+};
+
+void to_json(wpi::json& json, const Translation2d& state);
+
+void from_json(const wpi::json& json, Translation2d& state);
+
+}  // namespace frc

wpimath/src/main/native/include/frc/geometry/Twist2d.h

diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
new file mode 100644
index 0000000..b71ee56
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * A change in distance along arc since the last pose update. We can use ideas
+ * from differential calculus to create new Pose2ds from a Twist2d and vise
+ * versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct Twist2d {
+  /**
+   * Linear "dx" component
+   */
+  units::meter_t dx = 0_m;
+
+  /**
+   * Linear "dy" component
+   */
+  units::meter_t dy = 0_m;
+
+  /**
+   * Angular "dtheta" component (radians)
+   */
+  units::radian_t dtheta = 0_rad;
+
+  /**
+   * Checks equality between this Twist2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Twist2d& other) const {
+    return units::math::abs(dx - other.dx) < 1E-9_m &&
+           units::math::abs(dy - other.dy) < 1E-9_m &&
+           units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
+  }
+
+  /**
+   * Checks inequality between this Twist2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Twist2d& other) const { return !operator==(other); }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
new file mode 100644
index 0000000..1716ca7
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/geometry/Rotation2d.h"
+#include "units/angular_velocity.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents the speed of a robot chassis. Although this struct contains
+ * similar members compared to a Twist2d, they do NOT represent the same thing.
+ * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
+ * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
+ * frame of reference.
+ *
+ * A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have a dy component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+struct ChassisSpeeds {
+  /**
+   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+   */
+  units::meters_per_second_t vx = 0_mps;
+
+  /**
+   * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
+   */
+  units::meters_per_second_t vy = 0_mps;
+
+  /**
+   * Represents the angular velocity of the robot frame. (CCW is +)
+   */
+  units::radians_per_second_t omega = 0_rad_per_s;
+
+  /**
+   * Converts a user provided field-relative set of speeds into a robot-relative
+   * ChassisSpeeds object.
+   *
+   * @param vx The component of speed in the x direction relative to the field.
+   * Positive x is away from your alliance wall.
+   * @param vy The component of speed in the y direction relative to the field.
+   * Positive y is to your left when standing behind the alliance wall.
+   * @param omega The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   * robot's angle is considered to be zero when it is facing directly away from
+   * your alliance station wall. Remember that this should be CCW positive.
+   *
+   * @return ChassisSpeeds object representing the speeds in the robot's frame
+   * of reference.
+   */
+  static ChassisSpeeds FromFieldRelativeSpeeds(
+      units::meters_per_second_t vx, units::meters_per_second_t vy,
+      units::radians_per_second_t omega, const Rotation2d& robotAngle) {
+    return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
+            -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
+  }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
new file mode 100644
index 0000000..9e48b5e
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to
+ * left and right wheel velocities for a differential drive.
+ *
+ * Inverse kinematics converts a desired chassis speed into left and right
+ * velocity components whereas forward kinematics converts left and right
+ * component velocities into a linear and angular chassis speed.
+ */
+class DifferentialDriveKinematics {
+ public:
+  /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackWidth The track width of the drivetrain. Theoretically, this is
+   * the distance between the left wheels and right wheels. However, the
+   * empirical value may be larger than the physical measured value due to
+   * scrubbing effects.
+   */
+  explicit DifferentialDriveKinematics(units::meter_t trackWidth)
+      : trackWidth(trackWidth) {
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1);
+  }
+
+  /**
+   * Returns a chassis speed from left and right component velocities using
+   * forward kinematics.
+   *
+   * @param wheelSpeeds The left and right velocities.
+   * @return The chassis speed.
+   */
+  constexpr ChassisSpeeds ToChassisSpeeds(
+      const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+    return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
+            (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
+  }
+
+  /**
+   * Returns left and right component velocities from a chassis speed using
+   * inverse kinematics.
+   *
+   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
+   * represent the chassis' speed.
+   * @return The left and right velocities.
+   */
+  constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const {
+    return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
+            chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
+  }
+
+  units::meter_t trackWidth;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
new file mode 100644
index 0000000..a65b52a
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "DifferentialDriveKinematics.h"
+#include "frc/geometry/Pose2d.h"
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Class for differential drive odometry. Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from
+ * 2 encoders and a gyroscope.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ *
+ * It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
+ */
+class DifferentialDriveOdometry {
+ public:
+  /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
+                                     const Pose2d& initialPose = Pose2d());
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+    m_prevLeftDistance = 0_m;
+    m_prevRightDistance = 0_m;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot position on the field using distance measurements from
+   * encoders. This method is more numerically accurate than using velocities to
+   * integrate the pose and is also advantageous for teams that are using lower
+   * CPR encoders.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                       units::meter_t rightDistance);
+
+ private:
+  Pose2d m_pose;
+
+  Rotation2d m_gyroOffset;
+  Rotation2d m_previousAngle;
+
+  units::meter_t m_prevLeftDistance = 0_m;
+  units::meter_t m_prevRightDistance = 0_m;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
new file mode 100644
index 0000000..20085bb
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a differential drive drivetrain.
+ */
+struct DifferentialDriveWheelSpeeds {
+  /**
+   * Speed of the left side of the robot.
+   */
+  units::meters_per_second_t left = 0_mps;
+
+  /**
+   * Speed of the right side of the robot.
+   */
+  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.
+   *
+   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+   */
+  void Normalize(units::meters_per_second_t attainableMaxSpeed);
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
new file mode 100644
index 0000000..a0ccb52
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -0,0 +1,144 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds.
+ *
+ * The inverse kinematics (converting from a desired chassis velocity to
+ * individual wheel speeds) uses the relative locations of the wheels with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion maneuvers.
+ *
+ * Forward kinematics (converting an array of wheel speeds into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
+ * multiply by [wheelSpeeds] to get our chassis speeds.
+ *
+ * Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+class MecanumDriveKinematics {
+ public:
+  /**
+   * Constructs a mecanum drive kinematics object.
+   *
+   * @param frontLeftWheel The location of the front-left wheel relative to the
+   *                       physical center of the robot.
+   * @param frontRightWheel The location of the front-right wheel relative to
+   *                        the physical center of the robot.
+   * @param rearLeftWheel The location of the rear-left wheel relative to the
+   *                      physical center of the robot.
+   * @param rearRightWheel The location of the rear-right wheel relative to the
+   *                       physical center of the robot.
+   */
+  explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
+                                  Translation2d frontRightWheel,
+                                  Translation2d rearLeftWheel,
+                                  Translation2d rearRightWheel)
+      : m_frontLeftWheel{frontLeftWheel},
+        m_frontRightWheel{frontRightWheel},
+        m_rearLeftWheel{rearLeftWheel},
+        m_rearRightWheel{rearRightWheel} {
+    SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
+                         rearRightWheel);
+    m_forwardKinematics = m_inverseKinematics.householderQr();
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kKinematics_MecanumDrive, 1);
+  }
+
+  MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * wheel speeds.
+   *
+   * This function also supports variable centers of rotation. During normal
+   * operations, the center of rotation is usually the same as the physical
+   * center of the robot; therefore, the argument is defaulted to that use case.
+   * However, if you wish to change the center of rotation for evasive
+   * maneuvers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @param centerOfRotation The center of rotation. For example, if you set the
+   *                         center of rotation at one corner of the robot and
+   *                         provide a chassis speed that only has a dtheta
+   *                         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
+   *         MecanumDriveWheelSpeeds::Normalize() function to rectify this
+   *         issue. In addition, you can leverage the power of C++17 to directly
+   *         assign the wheel speeds to variables:
+   *
+   * @code{.cpp}
+   * auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
+   * @endcode
+   */
+  MecanumDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds,
+      const Translation2d& centerOfRotation = Translation2d()) const;
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the
+   * given wheel speeds. This method is often used for odometry -- determining
+   * the robot's position on the field using data from the real-world speed of
+   * each wheel on the robot.
+   *
+   * @param wheelSpeeds The current mecanum drive wheel speeds.
+   *
+   * @return The resulting chassis speed.
+   */
+  ChassisSpeeds ToChassisSpeeds(
+      const MecanumDriveWheelSpeeds& wheelSpeeds) const;
+
+ private:
+  mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
+  Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
+  Translation2d m_frontLeftWheel;
+  Translation2d m_frontRightWheel;
+  Translation2d m_rearLeftWheel;
+  Translation2d m_rearRightWheel;
+
+  mutable Translation2d m_previousCoR;
+
+  /**
+   * Construct inverse kinematics matrix from wheel locations.
+   *
+   * @param fl The location of the front-left wheel relative to the physical
+   *           center of the robot.
+   * @param fr The location of the front-right wheel relative to the physical
+   *           center of the robot.
+   * @param rl The location of the rear-left wheel relative to the physical
+   *           center of the robot.
+   * @param rr The location of the rear-right wheel relative to the physical
+   *           center of the robot.
+   */
+  void SetInverseKinematics(Translation2d fl, Translation2d fr,
+                            Translation2d rl, Translation2d rr) const;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
new file mode 100644
index 0000000..546a498
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/timestamp.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Class for mecanum drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * mecanum wheel encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+class MecanumDriveOdometry {
+ public:
+  /**
+   * Constructs a MecanumDriveOdometry object.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+                                const Rotation2d& gyroAngle,
+                                const Pose2d& initialPose = Pose2d());
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param currentTime The current time.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelSpeeds The current wheel speeds.
+   *
+   * @return The new pose of the robot.
+   */
+  const Pose2d& UpdateWithTime(units::second_t currentTime,
+                               const Rotation2d& gyroAngle,
+                               MecanumDriveWheelSpeeds wheelSpeeds);
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method automatically calculates
+   * the current time to calculate period (difference between two timestamps).
+   * The period is used to calculate the change in distance from a velocity.
+   * This also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelSpeeds The current wheel speeds.
+   *
+   * @return The new pose of the robot.
+   */
+  const Pose2d& Update(const Rotation2d& gyroAngle,
+                       MecanumDriveWheelSpeeds wheelSpeeds) {
+    return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
+  }
+
+ private:
+  MecanumDriveKinematics m_kinematics;
+  Pose2d m_pose;
+
+  units::second_t m_previousTime = -1_s;
+  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
new file mode 100644
index 0000000..aa82b99
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a mecanum drive drivetrain.
+ */
+struct MecanumDriveWheelSpeeds {
+  /**
+   * Speed of the front-left wheel.
+   */
+  units::meters_per_second_t frontLeft = 0_mps;
+
+  /**
+   * Speed of the front-right wheel.
+   */
+  units::meters_per_second_t frontRight = 0_mps;
+
+  /**
+   * Speed of the rear-left wheel.
+   */
+  units::meters_per_second_t rearLeft = 0_mps;
+
+  /**
+   * Speed of the rear-right wheel.
+   */
+  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.
+   *
+   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+   */
+  void Normalize(units::meters_per_second_t attainableMaxSpeed);
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
new file mode 100644
index 0000000..0ed50b4
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -0,0 +1,169 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <cstddef>
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/SwerveModuleState.h"
+#include "units/velocity.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual module states (speed and angle).
+ *
+ * The inverse kinematics (converting from a desired chassis velocity to
+ * individual module states) uses the relative locations of the modules with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion maneuvers.
+ *
+ * Forward kinematics (converting an array of module states into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
+ * multiply by [moduleStates] to get our chassis speeds.
+ *
+ * Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+template <size_t NumModules>
+class SwerveDriveKinematics {
+ public:
+  /**
+   * Constructs a swerve drive kinematics object. This takes in a variable
+   * number of wheel locations as Translation2ds. The order in which you pass in
+   * the wheel locations is the same order that you will receive the module
+   * states when performing inverse kinematics. It is also expected that you
+   * pass in the module states in the same order when calling the forward
+   * kinematics methods.
+   *
+   * @param wheels The locations of the wheels relative to the physical center
+   * of the robot.
+   */
+  template <typename... Wheels>
+  explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
+      : m_modules{wheel, wheels...} {
+    static_assert(sizeof...(wheels) >= 1,
+                  "A swerve drive requires at least two modules");
+
+    for (size_t i = 0; i < NumModules; i++) {
+      // clang-format off
+      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
+        1, 0, (-m_modules[i].Y()).template to<double>(),
+        0, 1, (+m_modules[i].X()).template to<double>();
+      // clang-format on
+    }
+
+    m_forwardKinematics = m_inverseKinematics.householderQr();
+
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
+  }
+
+  SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
+
+  /**
+   * Performs inverse kinematics to return the module states from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * module speeds and angles.
+   *
+   * This function also supports variable centers of rotation. During normal
+   * operations, the center of rotation is usually the same as the physical
+   * center of the robot; therefore, the argument is defaulted to that use case.
+   * However, if you wish to change the center of rotation for evasive
+   * maneuvers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @param centerOfRotation The center of rotation. For example, if you set the
+   * center of rotation at one corner of the robot and provide a chassis speed
+   * that only has a dtheta 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
+   * <NormalizeWheelSpeeds> function to rectify this issue. In addition, you can
+   * leverage the power of C++17 to directly assign the module states to
+   * variables:
+   *
+   * @code{.cpp}
+   * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
+   * @endcode
+   */
+  std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
+      const ChassisSpeeds& chassisSpeeds,
+      const Translation2d& centerOfRotation = Translation2d()) const;
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the
+   * given module states. This method is often used for odometry -- determining
+   * the robot's position on the field using data from the real-world speed and
+   * angle of each module on the robot.
+   *
+   * @param wheelStates The state of the modules (as a SwerveModuleState type)
+   * as measured from respective encoders and gyros. The order of the swerve
+   * module states should be same as passed into the constructor of this class.
+   *
+   * @return The resulting chassis speed.
+   */
+  template <typename... ModuleStates>
+  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
+
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the
+   * given module states. This method is often used for odometry -- determining
+   * the robot's position on the field using data from the real-world speed and
+   * angle of each module on the robot.
+   *
+   * @param moduleStates The state of the modules as an std::array of type
+   * SwerveModuleState, NumModules long as measured from respective encoders
+   * and gyros. The order of the swerve module states should be same as passed
+   * into the constructor of this class.
+   *
+   * @return The resulting chassis speed.
+   */
+  ChassisSpeeds ToChassisSpeeds(
+      std::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.
+   *
+   * @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(
+      std::array<SwerveModuleState, NumModules>* moduleStates,
+      units::meters_per_second_t attainableMaxSpeed);
+
+ private:
+  mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
+  Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
+      m_forwardKinematics;
+  std::array<Translation2d, NumModules> m_modules;
+
+  mutable Translation2d m_previousCoR;
+};
+}  // namespace frc
+
+#include "SwerveDriveKinematics.inc"

wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
new file mode 100644
index 0000000..08eba50
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+
+#include "units/math.h"
+
+namespace frc {
+
+template <class... Wheels>
+SwerveDriveKinematics(Translation2d, Wheels...)
+    -> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
+
+template <size_t NumModules>
+std::array<SwerveModuleState, NumModules>
+SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
+    const ChassisSpeeds& chassisSpeeds,
+    const Translation2d& centerOfRotation) const {
+  // We have a new center of rotation. We need to compute the matrix again.
+  if (centerOfRotation != m_previousCoR) {
+    for (size_t i = 0; i < NumModules; i++) {
+      // clang-format off
+      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
+        1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>(),
+        0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>();
+      // clang-format on
+    }
+    m_previousCoR = centerOfRotation;
+  }
+
+  Eigen::Vector3d chassisSpeedsVector;
+  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
+      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+
+  Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
+      m_inverseKinematics * chassisSpeedsVector;
+  std::array<SwerveModuleState, NumModules> moduleStates;
+
+  for (size_t i = 0; i < NumModules; i++) {
+    units::meters_per_second_t x =
+        units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)};
+    units::meters_per_second_t y =
+        units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)};
+
+    auto speed = units::math::hypot(x, y);
+    Rotation2d rotation{x.to<double>(), y.to<double>()};
+
+    moduleStates[i] = {speed, rotation};
+  }
+
+  return moduleStates;
+}
+
+template <size_t NumModules>
+template <typename... ModuleStates>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+    ModuleStates&&... wheelStates) const {
+  static_assert(sizeof...(wheelStates) == NumModules,
+                "Number of modules is not consistent with number of wheel "
+                "locations provided in constructor.");
+
+  std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+
+  return this->ToChassisSpeeds(moduleStates);
+}
+
+template <size_t NumModules>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+    std::array<SwerveModuleState, NumModules> moduleStates) const {
+  Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
+
+  for (size_t i = 0; i < NumModules; i++) {
+    SwerveModuleState module = moduleStates[i];
+    moduleStatesMatrix.row(i * 2)
+        << module.speed.to<double>() * module.angle.Cos();
+    moduleStatesMatrix.row(i * 2 + 1)
+        << module.speed.to<double>() * module.angle.Sin();
+  }
+
+  Eigen::Vector3d chassisSpeedsVector =
+      m_forwardKinematics.solve(moduleStatesMatrix);
+
+  return {units::meters_per_second_t{chassisSpeedsVector(0)},
+          units::meters_per_second_t{chassisSpeedsVector(1)},
+          units::radians_per_second_t{chassisSpeedsVector(2)}};
+}
+
+template <size_t NumModules>
+void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
+    std::array<SwerveModuleState, NumModules>* moduleStates,
+    units::meters_per_second_t attainableMaxSpeed) {
+  auto& states = *moduleStates;
+  auto realMaxSpeed = std::max_element(states.begin(), states.end(),
+                                       [](const auto& a, const auto& b) {
+                                         return units::math::abs(a.speed) <
+                                                units::math::abs(b.speed);
+                                       })
+                          ->speed;
+
+  if (realMaxSpeed > attainableMaxSpeed) {
+    for (auto& module : states) {
+      module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
+    }
+  }
+}
+
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
new file mode 100644
index 0000000..03591da
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <chrono>
+#include <cstddef>
+#include <ctime>
+
+#include <wpi/timestamp.h>
+
+#include "SwerveDriveKinematics.h"
+#include "SwerveModuleState.h"
+#include "frc/geometry/Pose2d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * swerve drive encoders and swerve azimuth encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+template <size_t NumModules>
+class SwerveDriveOdometry {
+ public:
+  /**
+   * Constructs a SwerveDriveOdometry object.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
+                      const Rotation2d& gyroAngle,
+                      const Pose2d& initialPose = Pose2d());
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param currentTime The current time.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param moduleStates The current state of all swerve modules. Please provide
+   *                     the states in the same order in which you instantiated
+   *                     your SwerveDriveKinematics.
+   *
+   * @return The new pose of the robot.
+   */
+  template <typename... ModuleStates>
+  const Pose2d& UpdateWithTime(units::second_t currentTime,
+                               const Rotation2d& gyroAngle,
+                               ModuleStates&&... moduleStates);
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method automatically calculates
+   * the current time to calculate period (difference between two timestamps).
+   * The period is used to calculate the change in distance from a velocity.
+   * This also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param moduleStates The current state of all swerve modules. Please provide
+   *                     the states in the same order in which you instantiated
+   *                     your SwerveDriveKinematics.
+   *
+   * @return The new pose of the robot.
+   */
+  template <typename... ModuleStates>
+  const Pose2d& Update(const Rotation2d& gyroAngle,
+                       ModuleStates&&... moduleStates) {
+    return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
+  }
+
+ private:
+  SwerveDriveKinematics<NumModules> m_kinematics;
+  Pose2d m_pose;
+
+  units::second_t m_previousTime = -1_s;
+  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
+};
+
+}  // namespace frc
+
+#include "SwerveDriveOdometry.inc"

wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
new file mode 100644
index 0000000..e7bb093
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "wpimath/MathShared.h"
+
+namespace frc {
+template <size_t NumModules>
+SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
+    SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+    const Pose2d& initialPose)
+    : m_kinematics(kinematics), m_pose(initialPose) {
+  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
+}
+
+template <size_t NumModules>
+template <typename... ModuleStates>
+const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    ModuleStates&&... moduleStates) {
+  units::second_t deltaTime =
+      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
+  m_previousTime = currentTime;
+
+  auto angle = gyroAngle + m_gyroOffset;
+
+  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
+  static_cast<void>(dtheta);
+
+  auto newPose = m_pose.Exp(
+      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+  m_previousAngle = angle;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}
+}  // namespace frc

wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
new file mode 100644
index 0000000..b5ae7f1
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/geometry/Rotation2d.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents the state of one swerve module.
+ */
+struct SwerveModuleState {
+  /**
+   * Speed of the wheel of the module.
+   */
+  units::meters_per_second_t speed = 0_mps;
+
+  /**
+   * Angle of the module.
+   */
+  Rotation2d angle;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h

diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
new file mode 100644
index 0000000..c9cf2d0
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include "Eigen/Core"
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 3.
+ */
+class CubicHermiteSpline : public Spline<3> {
+ public:
+  /**
+   * Constructs a cubic hermite spline with the specified control vectors. Each
+   * control vector contains info about the location of the point and its first
+   * derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   * the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   * the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   * the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   * the y dimension.
+   */
+  CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
+                     std::array<double, 2> xFinalControlVector,
+                     std::array<double, 2> yInitialControlVector,
+                     std::array<double, 2> yFinalControlVector);
+
+ protected:
+  /**
+   * Returns the coefficients matrix.
+   * @return The coefficients matrix.
+   */
+  Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
+    return m_coefficients;
+  }
+
+ private:
+  Eigen::Matrix<double, 6, 4> m_coefficients =
+      Eigen::Matrix<double, 6, 4>::Zero();
+
+  /**
+   * Returns the hermite basis matrix for cubic hermite spline interpolation.
+   * @return The hermite basis matrix for cubic hermite spline interpolation.
+   */
+  static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
+    // clang-format off
+    static auto basis = (Eigen::Matrix<double, 4, 4>() <<
+     +2.0, +1.0, -2.0, +1.0,
+     -3.0, -2.0, +3.0, -1.0,
+     +0.0, +1.0, +0.0, +0.0,
+     +1.0, +0.0, +0.0, +0.0).finished();
+    // clang-format on
+    return basis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   *
+   * @return The control vector matrix for a dimension.
+   */
+  static Eigen::Vector4d ControlVectorFromArrays(
+      std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
+    return (Eigen::Vector4d() << initialVector[0], initialVector[1],
+            finalVector[0], finalVector[1])
+        .finished();
+  }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h

diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
new file mode 100644
index 0000000..201c402
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include "Eigen/Core"
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 5.
+ */
+class QuinticHermiteSpline : public Spline<5> {
+ public:
+  /**
+   * Constructs a quintic hermite spline with the specified control vectors.
+   * Each control vector contains into about the location of the point, its
+   * first derivative, and its second derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   * the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   * the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   * the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   * the y dimension.
+   */
+  QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
+                       std::array<double, 3> xFinalControlVector,
+                       std::array<double, 3> yInitialControlVector,
+                       std::array<double, 3> yFinalControlVector);
+
+ protected:
+  /**
+   * Returns the coefficients matrix.
+   * @return The coefficients matrix.
+   */
+  Eigen::Matrix<double, 6, 6> Coefficients() const override {
+    return m_coefficients;
+  }
+
+ private:
+  Eigen::Matrix<double, 6, 6> m_coefficients =
+      Eigen::Matrix<double, 6, 6>::Zero();
+
+  /**
+   * Returns the hermite basis matrix for quintic hermite spline interpolation.
+   * @return The hermite basis matrix for quintic hermite spline interpolation.
+   */
+  static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
+    // clang-format off
+    static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
+      -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+      +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+      -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+      +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+      +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+      +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
+    // clang-format on
+    return basis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   *
+   * @return The control vector matrix for a dimension.
+   */
+  static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
+      std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
+    return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
+            initialVector[2], finalVector[0], finalVector[1], finalVector[2])
+        .finished();
+  }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/Spline.h

diff --git a/wpimath/src/main/native/include/frc/spline/Spline.h b/wpimath/src/main/native/include/frc/spline/Spline.h
new file mode 100644
index 0000000..2964476
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/spline/Spline.h
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <utility>
+#include <vector>
+
+#include "Eigen/Core"
+#include "frc/geometry/Pose2d.h"
+#include "units/curvature.h"
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents a two-dimensional parametric spline that interpolates between two
+ * points.
+ *
+ * @tparam Degree The degree of the spline.
+ */
+template <int Degree>
+class Spline {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
+
+  Spline() = default;
+
+  Spline(const Spline&) = default;
+  Spline& operator=(const Spline&) = default;
+
+  Spline(Spline&&) = default;
+  Spline& operator=(Spline&&) = default;
+
+  virtual ~Spline() = default;
+
+  /**
+   * Represents a control vector for a spline.
+   *
+   * Each element in each array represents the value of the derivative at the
+   * index. For example, the value of x[2] is the second derivative in the x
+   * dimension.
+   */
+  struct ControlVector {
+    std::array<double, (Degree + 1) / 2> x;
+    std::array<double, (Degree + 1) / 2> y;
+  };
+
+  /**
+   * Gets the pose and curvature at some point t on the spline.
+   *
+   * @param t The point t
+   * @return The pose and curvature at that point.
+   */
+  PoseWithCurvature GetPoint(double t) const {
+    Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
+
+    // Populate the polynomial bases
+    for (int i = 0; i <= Degree; i++) {
+      polynomialBases(i) = std::pow(t, Degree - i);
+    }
+
+    // This simply multiplies by the coefficients. We need to divide out t some
+    // n number of times where n is the derivative we want to take.
+    Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
+
+    double dx, dy, ddx, ddy;
+
+    // If t = 0, all other terms in the equation cancel out to zero. We can use
+    // the last x^0 term in the equation.
+    if (t == 0.0) {
+      dx = Coefficients()(2, Degree - 1);
+      dy = Coefficients()(3, Degree - 1);
+      ddx = Coefficients()(4, Degree - 2);
+      ddy = Coefficients()(5, Degree - 2);
+    } else {
+      // Divide out t for first derivative.
+      dx = combined(2) / t;
+      dy = combined(3) / t;
+
+      // Divide out t for second derivative.
+      ddx = combined(4) / t / t;
+      ddy = combined(5) / t / t;
+    }
+
+    // Find the curvature.
+    const auto curvature =
+        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
+
+    return {
+        {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
+        units::curvature_t(curvature)};
+  }
+
+ protected:
+  /**
+   * Returns the coefficients of the spline.
+   *
+   * @return The coefficients of the spline.
+   */
+  virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
+
+  /**
+   * Converts a Translation2d into a vector that is compatible with Eigen.
+   *
+   * @param translation The Translation2d to convert.
+   * @return The vector.
+   */
+  static Eigen::Vector2d ToVector(const Translation2d& translation) {
+    return (Eigen::Vector2d() << translation.X().to<double>(),
+            translation.Y().to<double>())
+        .finished();
+  }
+
+  /**
+   * Converts an Eigen vector into a Translation2d.
+   *
+   * @param vector The vector to convert.
+   * @return The Translation2d.
+   */
+  static Translation2d FromVector(const Eigen::Vector2d& vector) {
+    return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
+  }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/SplineHelper.h

diff --git a/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/wpimath/src/main/native/include/frc/spline/SplineHelper.h
new file mode 100644
index 0000000..e04fa45
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/spline/SplineHelper.h
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <utility>
+#include <vector>
+
+#include "frc/spline/CubicHermiteSpline.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+
+namespace frc {
+/**
+ * Helper class that is used to generate cubic and quintic splines from user
+ * provided waypoints.
+ */
+class SplineHelper {
+ public:
+  /**
+   * Returns 2 cubic control vectors from a set of exterior waypoints and
+   * interior translations.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @return 2 cubic control vectors.
+   */
+  static std::array<Spline<3>::ControlVector, 2>
+  CubicControlVectorsFromWaypoints(
+      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+      const Pose2d& end);
+
+  /**
+   * Returns quintic splines from a set of waypoints.
+   *
+   * @param waypoints The waypoints
+   * @return List of quintic splines.
+   */
+  static std::vector<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
+      const std::vector<Pose2d>& waypoints);
+
+  /**
+   * Returns a set of cubic splines corresponding to the provided control
+   * vectors. The user is free to set the direction of the start and end
+   * point. The directions for the middle waypoints are determined
+   * automatically to ensure continuous curvature throughout the path.
+   *
+   * The derivation for the algorithm used can be found here:
+   * <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
+   *
+   * @param start The starting control vector.
+   * @param waypoints The middle waypoints. This can be left blank if you
+   * only wish to create a path with two waypoints.
+   * @param end The ending control vector.
+   *
+   * @return A vector of cubic hermite splines that interpolate through the
+   * provided waypoints.
+   */
+  static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
+      const Spline<3>::ControlVector& start,
+      std::vector<Translation2d> waypoints,
+      const Spline<3>::ControlVector& end);
+
+  /**
+   * Returns a set of quintic splines corresponding to the provided control
+   * vectors. The user is free to set the direction of all waypoints. Continuous
+   * curvature is guaranteed throughout the path.
+   *
+   * @param controlVectors The control vectors.
+   * @return A vector of quintic hermite splines that interpolate through the
+   * provided waypoints.
+   */
+  static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
+      const std::vector<Spline<5>::ControlVector>& controlVectors);
+
+ private:
+  static Spline<3>::ControlVector CubicControlVector(double scalar,
+                                                     const Pose2d& point) {
+    return {{point.X().to<double>(), scalar * point.Rotation().Cos()},
+            {point.Y().to<double>(), scalar * point.Rotation().Sin()}};
+  }
+
+  static Spline<5>::ControlVector QuinticControlVector(double scalar,
+                                                       const Pose2d& point) {
+    return {{point.X().to<double>(), scalar * point.Rotation().Cos(), 0.0},
+            {point.Y().to<double>(), scalar * point.Rotation().Sin(), 0.0}};
+  }
+
+  /**
+   * Thomas algorithm for solving tridiagonal systems Af = d.
+   *
+   * @param a the values of A above the diagonal
+   * @param b the values of A on the diagonal
+   * @param c the values of A below the diagonal
+   * @param d the vector on the rhs
+   * @param solutionVector the unknown (solution) vector, modified in-place
+   */
+  static void ThomasAlgorithm(const std::vector<double>& a,
+                              const std::vector<double>& b,
+                              const std::vector<double>& c,
+                              const std::vector<double>& d,
+                              std::vector<double>* solutionVector);
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/SplineParameterizer.h

diff --git a/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
new file mode 100644
index 0000000..8e7079c
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <stack>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <wpi/Twine.h>
+
+#include "frc/spline/Spline.h"
+#include "units/angle.h"
+#include "units/curvature.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Class used to parameterize a spline by its arc length.
+ */
+class SplineParameterizer {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
+
+  struct MalformedSplineException : public std::runtime_error {
+    explicit MalformedSplineException(const char* what_arg)
+        : runtime_error(what_arg) {}
+  };
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various
+   * arcs until their dx, dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @param t0 Starting internal spline parameter. It is recommended to leave
+   * this as default.
+   * @param t1 Ending internal spline parameter. It is recommended to leave this
+   * as default.
+   *
+   * @return A vector of poses and curvatures that represents various points on
+   * the spline.
+   */
+  template <int Dim>
+  static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
+                                                     double t0 = 0.0,
+                                                     double t1 = 1.0) {
+    std::vector<PoseWithCurvature> splinePoints;
+
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.push_back(spline.GetPoint(t0));
+
+    // We use an "explicit stack" to simulate recursion, instead of a recursive
+    // function call This give us greater control, instead of a stack overflow
+    std::stack<StackContents> stack;
+    stack.emplace(StackContents{t0, t1});
+
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
+
+    while (!stack.empty()) {
+      current = stack.top();
+      stack.pop();
+      start = spline.GetPoint(current.t0);
+      end = spline.GetPoint(current.t1);
+
+      const auto twist = start.first.Log(end.first);
+
+      if (units::math::abs(twist.dy) > kMaxDy ||
+          units::math::abs(twist.dx) > kMaxDx ||
+          units::math::abs(twist.dtheta) > kMaxDtheta) {
+        stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
+        stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
+      } else {
+        splinePoints.push_back(spline.GetPoint(current.t1));
+      }
+
+      if (iterations++ >= kMaxIterations) {
+        throw MalformedSplineException(
+            "Could not parameterize a malformed spline. "
+            "This means that you probably had two or more adjacent "
+            "waypoints that were very close together with headings "
+            "in opposing directions.");
+      }
+    }
+
+    return splinePoints;
+  }
+
+ private:
+  // Constraints for spline parameterization.
+  static constexpr units::meter_t kMaxDx = 5_in;
+  static constexpr units::meter_t kMaxDy = 0.05_in;
+  static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
+
+  struct StackContents {
+    double t0;
+    double t1;
+  };
+
+  /**
+   * A malformed spline does not actually explode the LIFO stack size. Instead,
+   * the stack size stays at a relatively small number (e.g. 30) and never
+   * decreases. Because of this, we must count iterations. Even long, complex
+   * paths don't usually go over 300 iterations, so hitting this maximum should
+   * definitely indicate something has gone wrong.
+   */
+  static constexpr int kMaxIterations = 5000;
+
+  friend class CubicHermiteSplineTest;
+  friend class QuinticHermiteSplineTest;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/system/Discretization.h

diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h
new file mode 100644
index 0000000..72d0226
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/Discretization.h
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Eigen/Core"
+#include "Eigen/src/LU/PartialPivLU.h"
+#include "units/time.h"
+#include "unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h"
+
+namespace frc {
+
+/**
+ * Discretizes the given continuous A matrix.
+ *
+ * @param contA Continuous system matrix.
+ * @param dt    Discretization timestep.
+ * @param discA Storage for discrete system matrix.
+ */
+template <int States>
+void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
+                 units::second_t dt,
+                 Eigen::Matrix<double, States, States>* discA) {
+  *discA = (contA * dt.to<double>()).exp();
+}
+
+/**
+ * Discretizes the given continuous A and B matrices.
+ *
+ * @param contA Continuous system matrix.
+ * @param contB Continuous input matrix.
+ * @param dt    Discretization timestep.
+ * @param discA Storage for discrete system matrix.
+ * @param discB Storage for discrete input matrix.
+ */
+template <int States, int Inputs>
+void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
+                  const Eigen::Matrix<double, States, Inputs>& contB,
+                  units::second_t dt,
+                  Eigen::Matrix<double, States, States>* discA,
+                  Eigen::Matrix<double, States, Inputs>* discB) {
+  // Matrices are blocked here to minimize matrix exponentiation calculations
+  Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
+  Mcont.setZero();
+  Mcont.template block<States, States>(0, 0) = contA * dt.to<double>();
+  Mcont.template block<States, Inputs>(0, States) = contB * dt.to<double>();
+
+  // Discretize A and B with the given timestep
+  Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
+  *discA = Mdisc.template block<States, States>(0, 0);
+  *discB = Mdisc.template block<States, Inputs>(0, States);
+}
+
+/**
+ * Discretizes the given continuous A and Q matrices.
+ *
+ * @param contA Continuous system matrix.
+ * @param contQ Continuous process noise covariance matrix.
+ * @param dt    Discretization timestep.
+ * @param discA Storage for discrete system matrix.
+ * @param discQ Storage for discrete process noise covariance matrix.
+ */
+template <int States>
+void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
+                  const Eigen::Matrix<double, States, States>& contQ,
+                  units::second_t dt,
+                  Eigen::Matrix<double, States, States>* discA,
+                  Eigen::Matrix<double, States, States>* discQ) {
+  // Make continuous Q symmetric if it isn't already
+  Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
+
+  // Set up the matrix M = [[-A, Q], [0, A.T]]
+  Eigen::Matrix<double, 2 * States, 2 * States> M;
+  M.template block<States, States>(0, 0) = -contA;
+  M.template block<States, States>(0, States) = Q;
+  M.template block<States, States>(States, 0).setZero();
+  M.template block<States, States>(States, States) = contA.transpose();
+
+  Eigen::Matrix<double, 2 * States, 2 * States> phi =
+      (M * dt.to<double>()).exp();
+
+  // Phi12 = phi[0:States,        States:2*States]
+  // Phi22 = phi[States:2*States, States:2*States]
+  Eigen::Matrix<double, States, States> phi12 =
+      phi.block(0, States, States, States);
+  Eigen::Matrix<double, States, States> phi22 =
+      phi.block(States, States, States, States);
+
+  *discA = phi22.transpose();
+
+  Q = *discA * phi12;
+
+  // Make discrete Q symmetric if it isn't already
+  *discQ = (Q + Q.transpose()) / 2.0;
+}
+
+/**
+ * Discretizes the given continuous A and Q matrices.
+ *
+ * Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ()
+ * (which is expensive), we take advantage of the structure of the block matrix
+ * of A and Q.
+ *
+ * 1) The exponential of A*t, which is only N x N, is relatively cheap.
+ * 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
+ *    using a taylor series to several terms and still be substantially cheaper
+ *    than taking the big exponential.
+ *
+ * @param contA Continuous system matrix.
+ * @param contQ Continuous process noise covariance matrix.
+ * @param dt    Discretization timestep.
+ * @param discA Storage for discrete system matrix.
+ * @param discQ Storage for discrete process noise covariance matrix.
+ */
+template <int States>
+void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
+                        const Eigen::Matrix<double, States, States>& contQ,
+                        units::second_t dt,
+                        Eigen::Matrix<double, States, States>* discA,
+                        Eigen::Matrix<double, States, States>* discQ) {
+  // Make continuous Q symmetric if it isn't already
+  Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
+
+  Eigen::Matrix<double, States, States> lastTerm = Q;
+  double lastCoeff = dt.to<double>();
+
+  // A^T^n
+  Eigen::Matrix<double, States, States> Atn = contA.transpose();
+
+  Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
+
+  // i = 6 i.e. 5th order should be enough precision
+  for (int i = 2; i < 6; ++i) {
+    lastTerm = -contA * lastTerm + Q * Atn;
+    lastCoeff *= dt.to<double>() / static_cast<double>(i);
+
+    phi12 += lastTerm * lastCoeff;
+
+    Atn *= contA.transpose();
+  }
+
+  DiscretizeA<States>(contA, dt, discA);
+  Q = *discA * phi12;
+
+  // Make discrete Q symmetric if it isn't already
+  *discQ = (Q + Q.transpose()) / 2.0;
+}
+
+/**
+ * Returns a discretized version of the provided continuous measurement noise
+ * covariance matrix.
+ *
+ * @param R  Continuous measurement noise covariance matrix.
+ * @param dt Discretization timestep.
+ */
+template <int Outputs>
+Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
+    const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
+  return R / dt.to<double>();
+}
+
+}  // namespace frc

wpimath/src/main/native/include/frc/system/LinearSystem.h

diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h
new file mode 100644
index 0000000..975fa0e
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h
@@ -0,0 +1,164 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+#include <functional>
+
+#include "Eigen/Core"
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/Discretization.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * A plant defined using state-space notation.
+ *
+ * A plant is a mathematical model of a system's dynamics.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+template <int States, int Inputs, int Outputs>
+class LinearSystem {
+ public:
+  /**
+   * Constructs a discrete plant with the given continuous system coefficients.
+   *
+   * @param A    System matrix.
+   * @param B    Input matrix.
+   * @param C    Output matrix.
+   * @param D    Feedthrough matrix.
+   */
+  LinearSystem(const Eigen::Matrix<double, States, States>& A,
+               const Eigen::Matrix<double, States, Inputs>& B,
+               const Eigen::Matrix<double, Outputs, States>& C,
+               const Eigen::Matrix<double, Outputs, Inputs>& D) {
+    m_A = A;
+    m_B = B;
+    m_C = C;
+    m_D = D;
+  }
+
+  LinearSystem(const LinearSystem&) = default;
+  LinearSystem& operator=(const LinearSystem&) = default;
+  LinearSystem(LinearSystem&&) = default;
+  LinearSystem& operator=(LinearSystem&&) = default;
+
+  /**
+   * Returns the system matrix A.
+   */
+  const Eigen::Matrix<double, States, States>& A() const { return m_A; }
+
+  /**
+   * Returns an element of the system matrix A.
+   *
+   * @param i Row of A.
+   * @param j Column of A.
+   */
+  double A(int i, int j) const { return m_A(i, j); }
+
+  /**
+   * Returns the input matrix B.
+   */
+  const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
+
+  /**
+   * Returns an element of the input matrix B.
+   *
+   * @param i Row of B.
+   * @param j Column of B.
+   */
+  double B(int i, int j) const { return m_B(i, j); }
+
+  /**
+   * Returns the output matrix C.
+   */
+  const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
+
+  /**
+   * Returns an element of the output matrix C.
+   *
+   * @param i Row of C.
+   * @param j Column of C.
+   */
+  double C(int i, int j) const { return m_C(i, j); }
+
+  /**
+   * Returns the feedthrough matrix D.
+   */
+  const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
+
+  /**
+   * Returns an element of the feedthrough matrix D.
+   *
+   * @param i Row of D.
+   * @param j Column of D.
+   */
+  double D(int i, int j) const { return m_D(i, j); }
+
+  /**
+   * Computes the new x given the old x and the control input.
+   *
+   * This is used by state observers directly to run updates based on state
+   * estimate.
+   *
+   * @param x  The current state.
+   * @param u  The control input.
+   * @param dt Timestep for model update.
+   */
+  Eigen::Matrix<double, States, 1> CalculateX(
+      const Eigen::Matrix<double, States, 1>& x,
+      const Eigen::Matrix<double, Inputs, 1>& clampedU,
+      units::second_t dt) const {
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
+
+    return discA * x + discB * clampedU;
+  }
+
+  /**
+   * Computes the new y given the control input.
+   *
+   * This is used by state observers directly to run updates based on state
+   * estimate.
+   *
+   * @param x The current state.
+   * @param clampedU The control input.
+   */
+  Eigen::Matrix<double, Outputs, 1> CalculateY(
+      const Eigen::Matrix<double, States, 1>& x,
+      const Eigen::Matrix<double, Inputs, 1>& clampedU) const {
+    return m_C * x + m_D * clampedU;
+  }
+
+ private:
+  /**
+   * Continuous system matrix.
+   */
+  Eigen::Matrix<double, States, States> m_A;
+
+  /**
+   * Continuous input matrix.
+   */
+  Eigen::Matrix<double, States, Inputs> m_B;
+
+  /**
+   * Output matrix.
+   */
+  Eigen::Matrix<double, Outputs, States> m_C;
+
+  /**
+   * Feedthrough matrix.
+   */
+  Eigen::Matrix<double, Outputs, Inputs> m_D;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/system/LinearSystemLoop.h

diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
new file mode 100644
index 0000000..d5f25fb
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -0,0 +1,308 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/estimator/KalmanFilter.h"
+#include "frc/system/LinearSystem.h"
+#include "units/time.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Combines a plant, controller, and observer for controlling a mechanism with
+ * full state feedback.
+ *
+ * For everything in this file, "inputs" and "outputs" are defined from the
+ * perspective of the plant. This means U is an input and Y is an output
+ * (because you give the plant U (powers) and it gives you back a Y (sensor
+ * values). This is the opposite of what they mean from the perspective of the
+ * controller (U is an output because that's what goes to the motors and Y is an
+ * input because that's what comes back from the sensors).
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+template <int States, int Inputs, int Outputs>
+class LinearSystemLoop {
+ public:
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer. By default, the initial reference is all zeros. Users should
+   * call reset with the initial system state before enabling the loop. This
+   * constructor assumes that the input(s) to this system are voltage.
+   *
+   * @param plant      State-space plant.
+   * @param controller State-space controller.
+   * @param observer   State-space observer.
+   * @param maxVoltage The maximum voltage that can be applied. Commonly 12.
+   * @param dt         The nominal timestep.
+   */
+  LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
+                   LinearQuadraticRegulator<States, Inputs>& controller,
+                   KalmanFilter<States, Inputs, Outputs>& observer,
+                   units::volt_t maxVoltage, units::second_t dt)
+      : LinearSystemLoop(
+            plant, controller, observer,
+            [=](Eigen::Matrix<double, Inputs, 1> u) {
+              return frc::NormalizeInputVector<Inputs>(
+                  u, maxVoltage.template to<double>());
+            },
+            dt) {}
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer. By default, the initial reference is all zeros. Users should
+   * call reset with the initial system state before enabling the loop. This
+   * constructor assumes that the input(s) to this system are voltage.
+   *
+   * @param plant      State-space plant.
+   * @param controller State-space controller.
+   * @param observer   State-space observer.
+   * @param clampFunction The function used to clamp the input vector.
+   * @param dt         The nominal timestep.
+   */
+  LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
+                   LinearQuadraticRegulator<States, Inputs>& controller,
+                   KalmanFilter<States, Inputs, Outputs>& observer,
+                   std::function<Eigen::Matrix<double, Inputs, 1>(
+                       const Eigen::Matrix<double, Inputs, 1>&)>
+                       clampFunction,
+                   units::second_t dt)
+      : LinearSystemLoop(
+            plant, controller,
+            LinearPlantInversionFeedforward<States, Inputs>{plant, dt},
+            observer, clampFunction) {}
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer. By default, the initial reference is all zeros. Users should
+   * call reset with the initial system state before enabling the loop.
+   *
+   * @param plant       State-space plant.
+   * @param controller  State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer    State-space observer.
+   * @param maxVoltage  The maximum voltage that can be applied. Assumes
+   * that the inputs are voltages.
+   */
+  LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs>& plant,
+      LinearQuadraticRegulator<States, Inputs>& controller,
+      const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
+      KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
+      : LinearSystemLoop(plant, controller, feedforward, observer,
+                         [=](Eigen::Matrix<double, Inputs, 1> u) {
+                           return frc::NormalizeInputVector<Inputs>(
+                               u, maxVoltage.template to<double>());
+                         }) {}
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and
+   * observer.
+   *
+   * @param plant         State-space plant.
+   * @param controller    State-space controller.
+   * @param feedforward   Plant-inversion feedforward.
+   * @param observer      State-space observer.
+   * @param clampFunction The function used to clamp the input vector.
+   */
+  LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs>& plant,
+      LinearQuadraticRegulator<States, Inputs>& controller,
+      const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
+      KalmanFilter<States, Inputs, Outputs>& observer,
+      std::function<Eigen::Matrix<double, Inputs, 1>(
+          const Eigen::Matrix<double, Inputs, 1>&)>
+          clampFunction)
+      : m_plant(plant),
+        m_controller(controller),
+        m_feedforward(feedforward),
+        m_observer(observer),
+        m_clampFunc(clampFunction) {
+    m_nextR.setZero();
+    Reset(m_nextR);
+  }
+
+  /**
+   * Returns the observer's state estimate x-hat.
+   */
+  const Eigen::Matrix<double, States, 1>& Xhat() const {
+    return m_observer.Xhat();
+  }
+
+  /**
+   * Returns an element of the observer's state estimate x-hat.
+   *
+   * @param i Row of x-hat.
+   */
+  double Xhat(int i) const { return m_observer.Xhat(i); }
+
+  /**
+   * Returns the controller's next reference r.
+   */
+  const Eigen::Matrix<double, States, 1>& NextR() const { return m_nextR; }
+
+  /**
+   * Returns an element of the controller's next reference r.
+   *
+   * @param i Row of r.
+   */
+  double NextR(int i) const { return NextR()(i); }
+
+  /**
+   * Returns the controller's calculated control input u.
+   */
+  Eigen::Matrix<double, Inputs, 1> U() const {
+    return ClampInput(m_controller.U() + m_feedforward.Uff());
+  }
+
+  /**
+   * Returns an element of the controller's calculated control input u.
+   *
+   * @param i Row of u.
+   */
+  double U(int i) const { return U()(i); }
+
+  /**
+   * Set the initial state estimate x-hat.
+   *
+   * @param xHat The initial state estimate x-hat.
+   */
+  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) {
+    m_observer.SetXhat(xHat);
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param i     Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  void SetXhat(int i, double value) { m_observer.SetXhat(i, value); }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  void SetNextR(const Eigen::Matrix<double, States, 1>& nextR) {
+    m_nextR = nextR;
+  }
+
+  /**
+   * Return the plant used internally.
+   */
+  const LinearSystem<States, Inputs, Outputs>& Plant() const { return m_plant; }
+
+  /**
+   * Return the controller used internally.
+   */
+  const LinearQuadraticRegulator<States, Inputs>& Controller() const {
+    return m_controller;
+  }
+
+  /**
+   * Return the feedforward used internally.
+   *
+   * @return the feedforward used internally.
+   */
+  const LinearPlantInversionFeedforward<States, Inputs> Feedforward() const {
+    return m_feedforward;
+  }
+
+  /**
+   * Return the observer used internally.
+   */
+  const KalmanFilter<States, Inputs, Outputs>& Observer() const {
+    return m_observer;
+  }
+
+  /**
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
+   */
+  void Reset(Eigen::Matrix<double, States, 1> initialState) {
+    m_nextR.setZero();
+    m_controller.Reset();
+    m_feedforward.Reset(initialState);
+    m_observer.SetXhat(initialState);
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   */
+  const Eigen::Matrix<double, States, 1> Error() const {
+    return m_controller.R() - m_observer.Xhat();
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param y Measurement vector.
+   */
+  void Correct(const Eigen::Matrix<double, Outputs, 1>& y) {
+    m_observer.Correct(U(), y);
+  }
+
+  /**
+   * Sets new controller output, projects model forward, and runs observer
+   * prediction.
+   *
+   * After calling this, the user should send the elements of u to the
+   * actuators.
+   *
+   * @param dt Timestep for model update.
+   */
+  void Predict(units::second_t dt) {
+    Eigen::Matrix<double, Inputs, 1> u =
+        ClampInput(m_controller.Calculate(m_observer.Xhat(), m_nextR) +
+                   m_feedforward.Calculate(m_nextR));
+    m_observer.Predict(u, dt);
+  }
+
+  /**
+   * Clamps input vector between system's minimum and maximum allowable input.
+   *
+   * @param u Input vector to clamp.
+   * @return Clamped input vector.
+   */
+  Eigen::Matrix<double, Inputs, 1> ClampInput(
+      const Eigen::Matrix<double, Inputs, 1>& u) const {
+    return m_clampFunc(u);
+  }
+
+ protected:
+  LinearSystem<States, Inputs, Outputs>& m_plant;
+  LinearQuadraticRegulator<States, Inputs>& m_controller;
+  LinearPlantInversionFeedforward<States, Inputs> m_feedforward;
+  KalmanFilter<States, Inputs, Outputs>& m_observer;
+
+  /**
+   * Clamping function.
+   */
+  std::function<Eigen::Matrix<double, Inputs, 1>(
+      const Eigen::Matrix<double, Inputs, 1>&)>
+      m_clampFunc;
+
+  // Reference to go to in the next cycle (used by feedforward controller).
+  Eigen::Matrix<double, States, 1> m_nextR;
+
+  // These are accessible from non-templated subclasses.
+  static constexpr int kStates = States;
+  static constexpr int kInputs = Inputs;
+  static constexpr int kOutputs = Outputs;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/system/NumericalJacobian.h

diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
new file mode 100644
index 0000000..cbd6943
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Eigen/Core"
+
+namespace frc {
+
+/**
+ * Returns numerical Jacobian with respect to x for f(x).
+ *
+ * @tparam Rows Number of rows in result of f(x).
+ * @tparam Cols Number of columns in result of f(x).
+ * @param f     Vector-valued function from which to compute Jacobian.
+ * @param x     Vector argument.
+ */
+template <int Rows, int Cols, typename F>
+auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
+  constexpr double kEpsilon = 1e-5;
+  Eigen::Matrix<double, Rows, Cols> result;
+  result.setZero();
+
+  // It's more expensive, but +- epsilon will be more accurate
+  for (int i = 0; i < Cols; ++i) {
+    Eigen::Matrix<double, Cols, 1> dX_plus = x;
+    dX_plus(i) += kEpsilon;
+    Eigen::Matrix<double, Cols, 1> dX_minus = x;
+    dX_minus(i) -= kEpsilon;
+    result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
+  }
+
+  return result;
+}
+
+/**
+ * Returns numerical Jacobian with respect to x for f(x, u, ...).
+ *
+ * @tparam Rows    Number of rows in result of f(x, u, ...).
+ * @tparam States  Number of rows in x.
+ * @tparam Inputs  Number of rows in u.
+ * @tparam F       Function object type.
+ * @tparam Args... Remaining arguments to f(x, u, ...).
+ * @param f        Vector-valued function from which to compute Jacobian.
+ * @param x        State vector.
+ * @param u        Input vector.
+ */
+template <int Rows, int States, int Inputs, typename F, typename... Args>
+auto NumericalJacobianX(F&& f, const Eigen::Matrix<double, States, 1>& x,
+                        const Eigen::Matrix<double, Inputs, 1>& u,
+                        Args&&... args) {
+  return NumericalJacobian<Rows, States>(
+      [&](Eigen::Matrix<double, States, 1> x) { return f(x, u, args...); }, x);
+}
+
+/**
+ * Returns numerical Jacobian with respect to u for f(x, u, ...).
+ *
+ * @tparam Rows    Number of rows in result of f(x, u, ...).
+ * @tparam States  Number of rows in x.
+ * @tparam Inputs  Number of rows in u.
+ * @tparam F       Function object type.
+ * @tparam Args... Remaining arguments to f(x, u, ...).
+ * @param f        Vector-valued function from which to compute Jacobian.
+ * @param x        State vector.
+ * @param u        Input vector.
+ */
+template <int Rows, int States, int Inputs, typename F, typename... Args>
+auto NumericalJacobianU(F&& f, const Eigen::Matrix<double, States, 1>& x,
+                        const Eigen::Matrix<double, Inputs, 1>& u,
+                        Args&&... args) {
+  return NumericalJacobian<Rows, Inputs>(
+      [&](Eigen::Matrix<double, Inputs, 1> u) { return f(x, u, args...); }, u);
+}
+
+}  // namespace frc

wpimath/src/main/native/include/frc/system/RungeKutta.h

diff --git a/wpimath/src/main/native/include/frc/system/RungeKutta.h b/wpimath/src/main/native/include/frc/system/RungeKutta.h
new file mode 100644
index 0000000..a83cafc
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/RungeKutta.h
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Eigen/Core"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+ *
+ * @param f  The function to integrate. It must take one argument x.
+ * @param x  The initial value of x.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T>
+T RungeKutta(F&& f, T x, units::second_t dt) {
+  const auto halfDt = 0.5 * dt;
+  T k1 = f(x);
+  T k2 = f(x + k1 * halfDt.to<double>());
+  T k3 = f(x + k2 * halfDt.to<double>());
+  T k4 = f(x + k3 * dt.to<double>());
+  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments x and u.
+ * @param x  The initial value of x.
+ * @param u  The value u held constant over the integration period.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T, typename U>
+T RungeKutta(F&& f, T x, U u, units::second_t dt) {
+  const auto halfDt = 0.5 * dt;
+  T k1 = f(x, u);
+  T k2 = f(x + k1 * halfDt.to<double>(), u);
+  T k3 = f(x + k2 * halfDt.to<double>(), u);
+  T k4 = f(x + k3 * dt.to<double>(), u);
+  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments x and t.
+ * @param x  The initial value of x.
+ * @param t  The initial value of t.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T>
+T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
+  const auto halfDt = 0.5 * dt;
+  T k1 = f(t, x);
+  T k2 = f(t + halfDt, x + k1 / 2.0);
+  T k3 = f(t + halfDt, x + k2 / 2.0);
+  T k4 = f(t + dt, x + k3);
+
+  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+}  // namespace frc

wpimath/src/main/native/include/frc/system/plant/DCMotor.h

diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
new file mode 100644
index 0000000..d2a2ba8
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/angular_velocity.h"
+#include "units/current.h"
+#include "units/impedance.h"
+#include "units/torque.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * Holds the constants for a DC motor.
+ */
+class DCMotor {
+ public:
+  using radians_per_second_per_volt_t =
+      units::unit_t<units::compound_unit<units::radians_per_second,
+                                         units::inverse<units::volt>>>;
+  using newton_meters_per_ampere_t =
+      units::unit_t<units::compound_unit<units::newton_meters,
+                                         units::inverse<units::ampere>>>;
+
+  units::volt_t nominalVoltage;
+  units::newton_meter_t stallTorque;
+  units::ampere_t stallCurrent;
+  units::ampere_t freeCurrent;
+  units::radians_per_second_t freeSpeed;
+
+  // Resistance of motor
+  units::ohm_t R;
+
+  // Motor velocity constant
+  radians_per_second_per_volt_t Kv;
+
+  // Torque constant
+  newton_meters_per_ampere_t Kt;
+
+  /**
+   * Constructs a DC motor.
+   *
+   * @param nominalVoltage Voltage at which the motor constants were measured.
+   * @param stallTorque    Current draw when stalled.
+   * @param stallCurrent   Current draw when stalled.
+   * @param freeCurrent    Current draw under no load.
+   * @param freeSpeed      Angular velocity under no load.
+   * @param numMotors      Number of motors in a gearbox.
+   */
+  constexpr DCMotor(units::volt_t nominalVoltage,
+                    units::newton_meter_t stallTorque,
+                    units::ampere_t stallCurrent, units::ampere_t freeCurrent,
+                    units::radians_per_second_t freeSpeed, int numMotors = 1)
+      : nominalVoltage(nominalVoltage),
+        stallTorque(stallTorque * numMotors),
+        stallCurrent(stallCurrent),
+        freeCurrent(freeCurrent),
+        freeSpeed(freeSpeed),
+        R(nominalVoltage / stallCurrent),
+        Kv(freeSpeed / (nominalVoltage - R * freeCurrent)),
+        Kt(stallTorque * numMotors / stallCurrent) {}
+
+  /**
+   * Returns current drawn by motor with given speed and input voltage.
+   *
+   * @param speed        The current angular velocity of the motor.
+   * @param inputVoltage The voltage being applied to the motor.
+   */
+  constexpr units::ampere_t Current(units::radians_per_second_t speed,
+                                    units::volt_t inputVoltage) const {
+    return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
+  }
+
+  /**
+   * Returns instance of CIM.
+   */
+  static constexpr DCMotor CIM(int numMotors = 1) {
+    return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of MiniCIM.
+   */
+  static constexpr DCMotor MiniCIM(int numMotors = 1) {
+    return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of Bag motor.
+   */
+  static constexpr DCMotor Bag(int numMotors = 1) {
+    return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of Vex 775 Pro.
+   */
+  static constexpr DCMotor Vex775Pro(int numMotors = 1) {
+    return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of Andymark RS 775-125.
+   */
+  static constexpr DCMotor RS775_125(int numMotors = 1) {
+    return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of Banebots RS 775.
+   */
+  static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
+    return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of Andymark 9015.
+   */
+  static constexpr DCMotor Andymark9015(int numMotors = 1) {
+    return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of Banebots RS 550.
+   */
+  static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
+    return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of NEO brushless motor.
+   */
+  static constexpr DCMotor NEO(int numMotors = 1) {
+    return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of NEO 550 brushless motor.
+   */
+  static constexpr DCMotor NEO550(int numMotors = 1) {
+    return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
+  }
+
+  /**
+   * Returns instance of Falcon 500 brushless motor.
+   */
+  static constexpr DCMotor Falcon500(int numMotors = 1) {
+    return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
+  }
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
new file mode 100644
index 0000000..b712460
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -0,0 +1,250 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+#include "units/acceleration.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+#include "units/moment_of_inertia.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+class LinearSystemId {
+ public:
+  template <typename Distance>
+  using Velocity_t = units::unit_t<
+      units::compound_unit<Distance, units::inverse<units::seconds>>>;
+
+  template <typename Distance>
+  using Acceleration_t = units::unit_t<units::compound_unit<
+      units::compound_unit<Distance, units::inverse<units::seconds>>,
+      units::inverse<units::seconds>>>;
+
+  /**
+   * Constructs the state-space model for an elevator.
+   *
+   * States: [[position], [velocity]]
+   * Inputs: [[voltage]]
+   * Outputs: [[position]]
+   *
+   * @param motor Instance of DCMotor.
+   * @param m Carriage mass.
+   * @param r Pulley radius.
+   * @param G Gear ratio from motor to carriage.
+   */
+  static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
+                                              units::kilogram_t m,
+                                              units::meter_t r, double G) {
+    auto A = frc::MakeMatrix<2, 2>(
+        0.0, 1.0, 0.0,
+        (-std::pow(G, 2) * motor.Kt /
+         (motor.R * units::math::pow<2>(r) * m * motor.Kv))
+            .to<double>());
+    auto B = frc::MakeMatrix<2, 1>(
+        0.0, (G * motor.Kt / (motor.R * r * m)).to<double>());
+    auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
+    auto D = frc::MakeMatrix<1, 1>(0.0);
+
+    return LinearSystem<2, 1, 1>(A, B, C, D);
+  }
+
+  /**
+   * Constructs the state-space model for a single-jointed arm.
+   *
+   * States: [[angle], [angular velocity]]
+   * Inputs: [[voltage]]
+   * Outputs: [[angle]]
+   *
+   * @param motor Instance of DCMotor.
+   * @param J Moment of inertia.
+   * @param G Gear ratio from motor to carriage.
+   */
+  static LinearSystem<2, 1, 1> SingleJointedArmSystem(
+      DCMotor motor, units::kilogram_square_meter_t J, double G) {
+    auto A = frc::MakeMatrix<2, 2>(
+        0.0, 1.0, 0.0,
+        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
+    auto B =
+        frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to<double>());
+    auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
+    auto D = frc::MakeMatrix<1, 1>(0.0);
+
+    return LinearSystem<2, 1, 1>(A, B, C, D);
+  }
+
+  /**
+   * Constructs the state-space model for a 1 DOF velocity-only system from
+   * system identification data.
+   *
+   * You MUST use an SI unit (i.e. meters or radians) for the Distance template
+   * argument. You may still use non-SI units (such as feet or inches) for the
+   * actual method arguments; they will automatically be converted to SI
+   * internally.
+   *
+   * States: [[velocity]]
+   * Inputs: [[voltage]]
+   * Outputs: [[velocity]]
+   *
+   * The parameters provided by the user are from this feedforward model:
+   *
+   * u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   */
+  template <typename Distance, typename = std::enable_if_t<
+                                   std::is_same_v<units::meter, Distance> ||
+                                   std::is_same_v<units::radian, Distance>>>
+  static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
+      decltype(1_V / Velocity_t<Distance>(1)) kV,
+      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    auto A = frc::MakeMatrix<1, 1>(-kV.template to<double>() /
+                                   kA.template to<double>());
+    auto B = frc::MakeMatrix<1, 1>(1.0 / kA.template to<double>());
+    auto C = frc::MakeMatrix<1, 1>(1.0);
+    auto D = frc::MakeMatrix<1, 1>(0.0);
+
+    return LinearSystem<1, 1, 1>(A, B, C, D);
+  }
+
+  /**
+   * Constructs the state-space model for a 1 DOF position system from system
+   * identification data.
+   *
+   * You MUST use an SI unit (i.e. meters or radians) for the Distance template
+   * argument. You may still use non-SI units (such as feet or inches) for the
+   * actual method arguments; they will automatically be converted to SI
+   * internally.
+   *
+   * States: [[position], [velocity]]
+   * Inputs: [[voltage]]
+   * Outputs: [[position]]
+   *
+   * The parameters provided by the user are from this feedforward model:
+   *
+   * u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   */
+  template <typename Distance, typename = std::enable_if_t<
+                                   std::is_same_v<units::meter, Distance> ||
+                                   std::is_same_v<units::radian, Distance>>>
+  static LinearSystem<2, 1, 1> IdentifyPositionSystem(
+      decltype(1_V / Velocity_t<Distance>(1)) kV,
+      decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    auto A = frc::MakeMatrix<2, 2>(
+        0.0, 1.0, 0.0, -kV.template to<double>() / kA.template to<double>());
+    auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA.template to<double>());
+    auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
+    auto D = frc::MakeMatrix<1, 1>(0.0);
+
+    return LinearSystem<2, 1, 1>(A, B, C, D);
+  }
+
+  /**
+   * Constructs the state-space model for a 2 DOF drivetrain velocity system
+   * from system identification data.
+   *
+   * States: [[left velocity], [right velocity]]
+   * Inputs: [[left voltage], [right voltage]]
+   * Outputs: [[left velocity], [right velocity]]
+   *
+   * @param kVlinear The linear velocity gain, in volt seconds per distance.
+   * @param kAlinear The linear acceleration gain, in volt seconds^2 per
+   * distance.
+   * @param kVangular The angular velocity gain, in volt seconds per angle.
+   * @param kAangular The angular acceleration gain, in volt seconds^2 per
+   * angle.
+   */
+  static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
+      decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
+      decltype(1_V / 1_rad_per_s) kVangular,
+      decltype(1_V / 1_rad_per_s_sq) kAangular) {
+    double c = 0.5 / (kAlinear.to<double>() * kAangular.to<double>());
+    double A1 = c * (-kAlinear.to<double>() * kVangular.to<double>() -
+                     kVlinear.to<double>() * kAangular.to<double>());
+    double A2 = c * (kAlinear.to<double>() * kVangular.to<double>() -
+                     kVlinear.to<double>() * kAangular.to<double>());
+    double B1 = c * (kAlinear.to<double>() + kAangular.to<double>());
+    double B2 = c * (kAangular.to<double>() - kAlinear.to<double>());
+
+    auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
+    auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
+    auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
+    auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
+
+    return LinearSystem<2, 2, 2>(A, B, C, D);
+  }
+
+  /**
+   * Constructs the state-space model for a flywheel.
+   *
+   * States: [[angular velocity]]
+   * Inputs: [[voltage]]
+   * Outputs: [[angular velocity]]
+   *
+   * @param motor Instance of DCMotor.
+   * @param J Moment of inertia.
+   * @param G Gear ratio from motor to carriage.
+   */
+  static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
+                                              units::kilogram_square_meter_t J,
+                                              double G) {
+    auto A = frc::MakeMatrix<1, 1>(
+        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
+    auto B = frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to<double>());
+    auto C = frc::MakeMatrix<1, 1>(1.0);
+    auto D = frc::MakeMatrix<1, 1>(0.0);
+
+    return LinearSystem<1, 1, 1>(A, B, C, D);
+  }
+
+  /**
+   * Constructs the state-space model for a drivetrain.
+   *
+   * States: [[left velocity], [right velocity]]
+   * Inputs: [[left voltage], [right voltage]]
+   * Outputs: [[left velocity], [right velocity]]
+   *
+   * @param motor Instance of DCMotor.
+   * @param m Drivetrain mass.
+   * @param r Wheel radius.
+   * @param rb Robot radius.
+   * @param G Gear ratio from motor to wheel.
+   * @param J Moment of inertia.
+   */
+  static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
+      const DCMotor& motor, units::kilogram_t m, units::meter_t r,
+      units::meter_t rb, units::kilogram_square_meter_t J, double G) {
+    auto C1 = -std::pow(G, 2) * motor.Kt /
+              (motor.Kv * motor.R * units::math::pow<2>(r));
+    auto C2 = G * motor.Kt / (motor.R * r);
+
+    auto A = frc::MakeMatrix<2, 2>(
+        ((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
+        ((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
+        ((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
+        ((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>());
+    auto B = frc::MakeMatrix<2, 2>(
+        ((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
+        ((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
+        ((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
+        ((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>());
+    auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
+    auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
+
+    return LinearSystem<2, 2, 2>(A, B, C, D);
+  }
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/Trajectory.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
new file mode 100644
index 0000000..023b2c2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -0,0 +1,171 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "units/acceleration.h"
+#include "units/curvature.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of
+ * various States that represent the pose, curvature, time elapsed, velocity,
+ * and acceleration at that point.
+ */
+class Trajectory {
+ public:
+  /**
+   * Represents one point on the trajectory.
+   */
+  struct State {
+    // The time elapsed since the beginning of the trajectory.
+    units::second_t t = 0_s;
+
+    // The speed at that point of the trajectory.
+    units::meters_per_second_t velocity = 0_mps;
+
+    // The acceleration at that point of the trajectory.
+    units::meters_per_second_squared_t acceleration = 0_mps_sq;
+
+    // The pose at that point of the trajectory.
+    Pose2d pose;
+
+    // The curvature at that point of the trajectory.
+    units::curvature_t curvature{0.0};
+
+    /**
+     * Checks equality between this State and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are equal.
+     */
+    bool operator==(const State& other) const;
+
+    /**
+     * Checks inequality between this State and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are not equal.
+     */
+    bool operator!=(const State& other) const;
+
+    /**
+     * Interpolates between two States.
+     *
+     * @param endValue The end value for the interpolation.
+     * @param i The interpolant (fraction).
+     *
+     * @return The interpolated state.
+     */
+    State Interpolate(State endValue, double i) const;
+  };
+
+  Trajectory() = default;
+
+  /**
+   * Constructs a trajectory from a vector of states.
+   */
+  explicit Trajectory(const std::vector<State>& states);
+
+  /**
+   * Returns the overall duration of the trajectory.
+   * @return The duration of the trajectory.
+   */
+  units::second_t TotalTime() const { return m_totalTime; }
+
+  /**
+   * Return the states of the trajectory.
+   * @return The states of the trajectory.
+   */
+  const std::vector<State>& States() const { return m_states; }
+
+  /**
+   * Sample the trajectory at a point in time.
+   *
+   * @param t The point in time since the beginning of the trajectory to sample.
+   * @return The state at that point in time.
+   */
+  State Sample(units::second_t t) const;
+
+  /**
+   * Transforms all poses in the trajectory by the given transform. This is
+   * useful for converting a robot-relative trajectory into a field-relative
+   * trajectory. This works with respect to the first pose in the trajectory.
+   *
+   * @param transform The transform to transform the trajectory by.
+   * @return The transformed trajectory.
+   */
+  Trajectory TransformBy(const Transform2d& transform);
+
+  /**
+   * Transforms all poses in the trajectory so that they are relative to the
+   * given pose. This is useful for converting a field-relative trajectory
+   * into a robot-relative trajectory.
+   *
+   * @param pose The pose that is the origin of the coordinate frame that
+   *             the current trajectory will be transformed into.
+   * @return The transformed trajectory.
+   */
+  Trajectory RelativeTo(const Pose2d& pose);
+
+  /**
+   * Returns the initial pose of the trajectory.
+   *
+   * @return The initial pose of the trajectory.
+   */
+  Pose2d InitialPose() const { return Sample(0_s).pose; }
+
+  /**
+   * Checks equality between this Trajectory and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Trajectory& other) const;
+
+  /**
+   * Checks inequality between this Trajectory and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are inequal.
+   */
+  bool operator!=(const Trajectory& other) const;
+
+ 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;
+  }
+};
+
+void to_json(wpi::json& json, const Trajectory::State& state);
+
+void from_json(const wpi::json& json, Trajectory::State& state);
+
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
new file mode 100644
index 0000000..5bd7977
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -0,0 +1,165 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/acceleration.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents the configuration for generating a trajectory. This class stores
+ * the start velocity, end velocity, max velocity, max acceleration, custom
+ * constraints, and the reversed flag.
+ *
+ * The class must be constructed with a max velocity and max acceleration.
+ * The other parameters (start velocity, end velocity, constraints, reversed)
+ * have been defaulted to reasonable values (0, 0, {}, false). These values can
+ * be changed via the SetXXX methods.
+ */
+class TrajectoryConfig {
+ public:
+  /**
+   * Constructs a config object.
+   * @param maxVelocity The max velocity of the trajectory.
+   * @param maxAcceleration The max acceleration of the trajectory.
+   */
+  TrajectoryConfig(units::meters_per_second_t maxVelocity,
+                   units::meters_per_second_squared_t maxAcceleration)
+      : m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
+
+  TrajectoryConfig(const TrajectoryConfig&) = delete;
+  TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
+
+  TrajectoryConfig(TrajectoryConfig&&) = default;
+  TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
+
+  /**
+   * Sets the start velocity of the trajectory.
+   * @param startVelocity The start velocity of the trajectory.
+   */
+  void SetStartVelocity(units::meters_per_second_t startVelocity) {
+    m_startVelocity = startVelocity;
+  }
+
+  /**
+   * Sets the end velocity of the trajectory.
+   * @param endVelocity The end velocity of the trajectory.
+   */
+  void SetEndVelocity(units::meters_per_second_t endVelocity) {
+    m_endVelocity = endVelocity;
+  }
+
+  /**
+   * Sets the reversed flag of the trajectory.
+   * @param reversed Whether the trajectory should be reversed or not.
+   */
+  void SetReversed(bool reversed) { m_reversed = reversed; }
+
+  /**
+   * Adds a user-defined constraint to the trajectory.
+   * @param constraint The user-defined constraint.
+   */
+  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
+                                     TrajectoryConstraint, Constraint>>>
+  void AddConstraint(Constraint constraint) {
+    m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
+  }
+
+  /**
+   * Adds a differential drive kinematics constraint to ensure that
+   * no wheel velocity of a differential drive goes above the max velocity.
+   *
+   * @param kinematics The differential drive kinematics.
+   */
+  void SetKinematics(const DifferentialDriveKinematics& kinematics) {
+    AddConstraint(
+        DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
+  /**
+   * Adds a mecanum drive kinematics constraint to ensure that
+   * no wheel velocity of a mecanum drive goes above the max velocity.
+   *
+   * @param kinematics The mecanum drive kinematics.
+   */
+  void SetKinematics(MecanumDriveKinematics kinematics) {
+    AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
+  /**
+   * Adds a swerve drive kinematics constraint to ensure that
+   * no wheel velocity of a swerve drive goes above the max velocity.
+   *
+   * @param kinematics The swerve drive kinematics.
+   */
+  template <size_t NumModules>
+  void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
+    AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   * @return The starting velocity of the trajectory.
+   */
+  units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
+
+  /**
+   * Returns the ending velocity of the trajectory.
+   * @return The ending velocity of the trajectory.
+   */
+  units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
+
+  /**
+   * Returns the maximum velocity of the trajectory.
+   * @return The maximum velocity of the trajectory.
+   */
+  units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
+
+  /**
+   * Returns the maximum acceleration of the trajectory.
+   * @return The maximum acceleration of the trajectory.
+   */
+  units::meters_per_second_squared_t MaxAcceleration() const {
+    return m_maxAcceleration;
+  }
+
+  /**
+   * Returns the user-defined constraints of the trajectory.
+   * @return The user-defined constraints of the trajectory.
+   */
+  const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
+      const {
+    return m_constraints;
+  }
+
+  /**
+   * Returns whether the trajectory is reversed or not.
+   * @return whether the trajectory is reversed or not.
+   */
+  bool IsReversed() const { return m_reversed; }
+
+ private:
+  units::meters_per_second_t m_startVelocity = 0_mps;
+  units::meters_per_second_t m_endVelocity = 0_mps;
+  units::meters_per_second_t m_maxVelocity;
+  units::meters_per_second_squared_t m_maxAcceleration;
+  std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
+  bool m_reversed = false;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
new file mode 100644
index 0000000..f1747fd
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/spline/SplineParameterizer.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Helper class used to generate trajectories with various constraints.
+ */
+class TrajectoryGenerator {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
+
+  /**
+   * Generates a trajectory from the given control vectors and config. This
+   * method uses clamped cubic splines -- a method in which the exterior control
+   * vectors and interior waypoints are provided. The headings are automatically
+   * determined at the interior points to ensure continuous curvature.
+   *
+   * @param initial           The initial control vector.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending control vector.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  static Trajectory GenerateTrajectory(
+      Spline<3>::ControlVector initial,
+      const std::vector<Translation2d>& interiorWaypoints,
+      Spline<3>::ControlVector end, const TrajectoryConfig& config);
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method
+   * uses clamped cubic splines -- a method in which the initial pose, final
+   * pose, and interior waypoints are provided.  The headings are automatically
+   * determined at the interior points to ensure continuous curvature.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @param config            The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  static Trajectory GenerateTrajectory(
+      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+      const Pose2d& end, const TrajectoryConfig& config);
+
+  /**
+   * Generates a trajectory from the given quintic control vectors and config.
+   * This method uses quintic hermite splines -- therefore, all points must be
+   * represented by control vectors. Continuous curvature is guaranteed in this
+   * method.
+   *
+   * @param controlVectors List of quintic control vectors.
+   * @param config         The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  static Trajectory GenerateTrajectory(
+      std::vector<Spline<5>::ControlVector> controlVectors,
+      const TrajectoryConfig& config);
+
+  /**
+   * Generates a trajectory from the given waypoints and config. This method
+   * uses quintic hermite splines -- therefore, all points must be represented
+   * by Pose2d objects. Continuous curvature is guaranteed in this method.
+   *
+   * @param waypoints List of waypoints..
+   * @param config    The configuration for the trajectory.
+   * @return The generated trajectory.
+   */
+  static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
+                                       const TrajectoryConfig& config);
+
+  /**
+   * Generate spline points from a vector of splines by parameterizing the
+   * splines.
+   *
+   * @param splines The splines to parameterize.
+   *
+   * @return The spline points for use in time parameterization of a trajectory.
+   */
+  template <typename Spline>
+  static std::vector<PoseWithCurvature> SplinePointsFromSplines(
+      const std::vector<Spline>& splines) {
+    // Create the vector of spline points.
+    std::vector<PoseWithCurvature> splinePoints;
+
+    // Add the first point to the vector.
+    splinePoints.push_back(splines.front().GetPoint(0.0));
+
+    // Iterate through the vector and parameterize each spline, adding the
+    // parameterized points to the final vector.
+    for (auto&& spline : splines) {
+      auto points = SplineParameterizer::Parameterize(spline);
+      // Append the array of poses to the vector. We are removing the first
+      // point because it's a duplicate of the last point from the previous
+      // spline.
+      splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
+                          std::end(points));
+    }
+    return splinePoints;
+  }
+
+  /**
+   * Set error reporting function. By default, it is output to stderr.
+   *
+   * @param func Error reporting function.
+   */
+  static void SetErrorHandler(std::function<void(const char*)> func) {
+    s_errorFunc = std::move(func);
+  }
+
+ private:
+  static void ReportError(const char* error);
+
+  static const Trajectory kDoNothingTrajectory;
+  static std::function<void(const char*)> s_errorFunc;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
new file mode 100644
index 0000000..378f007
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Class used to parameterize a trajectory by time.
+ */
+class TrajectoryParameterizer {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
+
+  /**
+   * Parameterize the trajectory by time. This is where the velocity profile is
+   * generated.
+   *
+   * The derivation of the algorithm used can be found here:
+   * <http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf>
+   *
+   * @param points Reference to the spline points.
+   * @param constraints A vector of various velocity and acceleration
+   * constraints.
+   * @param startVelocity The start velocity for the trajectory.
+   * @param endVelocity The end velocity for the trajectory.
+   * @param maxVelocity The max velocity for the trajectory.
+   * @param maxAcceleration The max acceleration for the trajectory.
+   * @param reversed Whether the robot should move backwards. Note that the
+   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
+   *
+   * @return The trajectory.
+   */
+  static Trajectory TimeParameterizeTrajectory(
+      const std::vector<PoseWithCurvature>& points,
+      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+      units::meters_per_second_t startVelocity,
+      units::meters_per_second_t endVelocity,
+      units::meters_per_second_t maxVelocity,
+      units::meters_per_second_squared_t maxAcceleration, bool reversed);
+
+ private:
+  constexpr static double kEpsilon = 1E-6;
+
+  /**
+   * Represents a constrained state that is used when time parameterizing a
+   * trajectory. Each state has the pose, curvature, distance from the start of
+   * the trajectory, max velocity, min acceleration and max acceleration.
+   */
+  struct ConstrainedState {
+    PoseWithCurvature pose = {Pose2d(), units::curvature_t(0.0)};
+    units::meter_t distance = 0_m;
+    units::meters_per_second_t maxVelocity = 0_mps;
+    units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
+    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+  };
+
+  /**
+   * Enforces acceleration limits as defined by the constraints. This function
+   * is used when time parameterizing a trajectory.
+   *
+   * @param reverse Whether the robot is traveling backwards.
+   * @param constraints A vector of the user-defined velocity and acceleration
+   * constraints.
+   * @param state Pointer to the constrained state that we are operating on.
+   * This is mutated in place.
+   */
+  static void EnforceAccelerationLimits(
+      bool reverse,
+      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+      ConstrainedState* state);
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
new file mode 100644
index 0000000..f05cd14
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+
+#include "frc/trajectory/Trajectory.h"
+
+namespace frc {
+class TrajectoryUtil {
+ public:
+  TrajectoryUtil() = delete;
+
+  /**
+   * Exports a Trajectory to a PathWeaver-style JSON file.
+   *
+   * @param trajectory the trajectory to export
+   * @param path the path of the file to export to
+   *
+   * @return The interpolated state.
+   */
+  static void ToPathweaverJson(const Trajectory& trajectory,
+                               const wpi::Twine& path);
+  /**
+   * Imports a Trajectory from a PathWeaver-style JSON file.
+   *
+   * @param path The path of the json file to import from.
+   *
+   * @return The trajectory represented by the file.
+   */
+  static Trajectory FromPathweaverJson(const wpi::Twine& path);
+
+  /**
+     * Deserializes a Trajectory from PathWeaver-style JSON.
+
+     * @param json the string containing the serialized JSON
+
+     * @return the trajectory represented by the JSON
+     */
+  static std::string SerializeTrajectory(const Trajectory& trajectory);
+
+  /**
+   * Serializes a Trajectory to PathWeaver-style JSON.
+
+   * @param trajectory the trajectory to export
+
+   * @return the string containing the serialized JSON
+   */
+  static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
new file mode 100644
index 0000000..5f9f1b8
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/time.h"
+#include "wpimath/MathShared.h"
+
+namespace frc {
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on trapezoidal
+ * velocity constraints. To compute the reference obeying this constraint, do
+ * the following.
+ *
+ * Initialization:
+ * @code{.cpp}
+ * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
+ * double previousProfiledReference = initialReference;
+ * @endcode
+ *
+ * Run on update:
+ * @code{.cpp}
+ * TrapezoidalMotionProfile profile{constraints, unprofiledReference,
+ *                                  previousProfiledReference};
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
+ * @endcode
+ *
+ * where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `Calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * Otherwise, a timer can be started to provide monotonic values for
+ * `Calculate()` and to determine when the profile has completed via
+ * `IsFinished()`.
+ */
+template <class Distance>
+class TrapezoidProfile {
+ public:
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+
+  class Constraints {
+   public:
+    Constraints() {
+      wpi::math::MathSharedStore::ReportUsage(
+          wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1);
+    }
+    Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
+        : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
+      wpi::math::MathSharedStore::ReportUsage(
+          wpi::math::MathUsageId::kTrajectory_TrapezoidProfile, 1);
+    }
+    Velocity_t maxVelocity{0};
+    Acceleration_t maxAcceleration{0};
+  };
+
+  class State {
+   public:
+    Distance_t position{0};
+    Velocity_t velocity{0};
+    bool operator==(const State& rhs) const {
+      return position == rhs.position && velocity == rhs.velocity;
+    }
+    bool operator!=(const State& rhs) const { return !(*this == rhs); }
+  };
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   * @param initial     The initial state (usually the current state).
+   */
+  TrapezoidProfile(Constraints constraints, State goal,
+                   State initial = State{Distance_t(0), Velocity_t(0)});
+
+  TrapezoidProfile(const TrapezoidProfile&) = default;
+  TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
+  TrapezoidProfile(TrapezoidProfile&&) = default;
+  TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  State Calculate(units::second_t t) const;
+
+  /**
+   * Returns the time left until a target distance in the profile is reached.
+   *
+   * @param target The target distance.
+   */
+  units::second_t TimeLeftUntil(Distance_t target) const;
+
+  /**
+   * Returns the total time the profile takes to reach the goal.
+   */
+  units::second_t TotalTime() const { return m_endDeccel; }
+
+  /**
+   * Returns true if the profile has reached the goal.
+   *
+   * The profile has reached the goal if the time since the profile started
+   * has exceeded the profile's total time.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
+
+ private:
+  /**
+   * Returns true if the profile inverted.
+   *
+   * The profile is inverted if goal position is less than the initial position.
+   *
+   * @param initial The initial state (usually the current state).
+   * @param goal    The desired state when the profile is complete.
+   */
+  static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
+    return initial.position > goal.position;
+  }
+
+  // Flip the sign of the velocity and position if the profile is inverted
+  State Direct(const State& in) const {
+    State result = in;
+    result.position *= m_direction;
+    result.velocity *= m_direction;
+    return result;
+  }
+
+  // The direction of the profile, either 1 for forwards or -1 for inverted
+  int m_direction;
+
+  Constraints m_constraints;
+  State m_initial;
+  State m_goal;
+
+  units::second_t m_endAccel;
+  units::second_t m_endFullSpeed;
+  units::second_t m_endDeccel;
+};
+}  // namespace frc
+
+#include "TrapezoidProfile.inc"

wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc

diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
new file mode 100644
index 0000000..8718cc0
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -0,0 +1,165 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+
+#include "units/math.h"
+
+namespace frc {
+template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
+                                             State goal, State initial)
+    : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
+      m_constraints(constraints),
+      m_initial(Direct(initial)),
+      m_goal(Direct(goal)) {
+  if (m_initial.velocity > m_constraints.maxVelocity) {
+    m_initial.velocity = m_constraints.maxVelocity;
+  }
+
+  // Deal with a possibly truncated motion profile (with nonzero initial or
+  // final velocity) by calculating the parameters as if the profile began and
+  // ended at zero velocity
+  units::second_t cutoffBegin =
+      m_initial.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistBegin =
+      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+  units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistEnd =
+      cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+  // Now we can calculate the parameters as if it was a full trapezoid instead
+  // of a truncated one
+
+  Distance_t fullTrapezoidDist =
+      cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+  units::second_t accelerationTime =
+      m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+  Distance_t fullSpeedDist =
+      fullTrapezoidDist -
+      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+  // Handle the case where the profile never reaches full speed
+  if (fullSpeedDist < Distance_t(0)) {
+    accelerationTime =
+        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+    fullSpeedDist = Distance_t(0);
+  }
+
+  m_endAccel = accelerationTime - cutoffBegin;
+  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+}
+
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
+  State result = m_initial;
+
+  if (t < m_endAccel) {
+    result.velocity += t * m_constraints.maxAcceleration;
+    result.position +=
+        (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+  } else if (t < m_endFullSpeed) {
+    result.velocity = m_constraints.maxVelocity;
+    result.position += (m_initial.velocity +
+                        m_endAccel * m_constraints.maxAcceleration / 2.0) *
+                           m_endAccel +
+                       m_constraints.maxVelocity * (t - m_endAccel);
+  } else if (t <= m_endDeccel) {
+    result.velocity =
+        m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+    units::second_t timeLeft = m_endDeccel - t;
+    result.position =
+        m_goal.position -
+        (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
+            timeLeft;
+  } else {
+    result = m_goal;
+  }
+
+  return Direct(result);
+}
+
+template <class Distance>
+units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
+    Distance_t target) const {
+  Distance_t position = m_initial.position * m_direction;
+  Velocity_t velocity = m_initial.velocity * m_direction;
+
+  units::second_t endAccel = m_endAccel * m_direction;
+  units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+  if (target < position) {
+    endAccel *= -1.0;
+    endFullSpeed *= -1.0;
+    velocity *= -1.0;
+  }
+
+  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;
+
+  Distance_t distToTarget = units::math::abs(target - position);
+
+  if (distToTarget < Distance_t(1e-6)) {
+    return 0_s;
+  }
+
+  Distance_t accelDist =
+      velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+  Velocity_t deccelVelocity;
+  if (endAccel > 0_s) {
+    deccelVelocity = units::math::sqrt(
+        units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
+  } else {
+    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;
+
+  if (accelDist > distToTarget) {
+    accelDist = distToTarget;
+    fullSpeedDist = Distance_t(0);
+    deccelDist = Distance_t(0);
+  } else if (accelDist + fullSpeedDist > distToTarget) {
+    fullSpeedDist = distToTarget - accelDist;
+    deccelDist = Distance_t(0);
+  } else {
+    deccelDist = distToTarget - fullSpeedDist - accelDist;
+  }
+
+  units::second_t accelTime =
+      (-velocity + units::math::sqrt(units::math::abs(
+                       velocity * velocity + 2 * acceleration * accelDist))) /
+      acceleration;
+
+  units::second_t deccelTime =
+      (-deccelVelocity +
+       units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
+                                          2 * decceleration * deccelDist))) /
+      decceleration;
+
+  units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+  return accelTime + fullSpeedTime + deccelTime;
+}
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
new file mode 100644
index 0000000..4f897ba
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/acceleration.h"
+#include "units/curvature.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * A constraint on the maximum absolute centripetal acceleration allowed when
+ * traversing a trajectory. The centripetal acceleration of a robot is defined
+ * as the velocity squared divided by the radius of curvature.
+ *
+ * Effectively, limiting the maximum centripetal acceleration will cause the
+ * robot to slow down around tight turns, making it easier to track trajectories
+ * with sharp turns.
+ */
+class CentripetalAccelerationConstraint : public TrajectoryConstraint {
+ public:
+  explicit CentripetalAccelerationConstraint(
+      units::meters_per_second_squared_t maxCentripetalAcceleration);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override;
+
+ private:
+  units::meters_per_second_squared_t m_maxCentripetalAcceleration;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
new file mode 100644
index 0000000..f23c1e2
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  DifferentialDriveKinematicsConstraint(
+      const DifferentialDriveKinematics& kinematics,
+      units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override;
+
+ private:
+  const DifferentialDriveKinematics& m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
new file mode 100644
index 0000000..23c690d
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/length.h"
+#include "units/voltage.h"
+
+namespace frc {
+/**
+ * A class that enforces constraints on differential drive voltage expenditure
+ * based on the motor dynamics and the drive kinematics.  Ensures that the
+ * acceleration of any wheel of the robot while following the trajectory is
+ * never higher than what can be achieved with the given maximum voltage.
+ */
+class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Creates a new DifferentialDriveVoltageConstraint.
+   *
+   * @param feedforward A feedforward component describing the behavior of the
+   * drive.
+   * @param kinematics  A kinematics component describing the drive geometry.
+   * @param maxVoltage  The maximum voltage available to the motors while
+   * following the path. Should be somewhat less than the nominal battery
+   * voltage (12V) to account for "voltage sag" due to current draw.
+   */
+  DifferentialDriveVoltageConstraint(
+      const SimpleMotorFeedforward<units::meter>& feedforward,
+      const DifferentialDriveKinematics& kinematics, units::volt_t maxVoltage);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override;
+
+ private:
+  const SimpleMotorFeedforward<units::meter>& m_feedforward;
+  const DifferentialDriveKinematics& m_kinematics;
+  units::volt_t m_maxVoltage;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
new file mode 100644
index 0000000..78bc569
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <limits>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Enforces a particular constraint only within an elliptical region.
+ */
+template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
+                                   TrajectoryConstraint, Constraint>>>
+class EllipticalRegionConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Constructs a new EllipticalRegionConstraint.
+   *
+   * @param center The center of the ellipse in which to enforce the constraint.
+   * @param xWidth The width of the ellipse in which to enforce the constraint.
+   * @param yWidth The height of the ellipse in which to enforce the constraint.
+   * @param rotation The rotation to apply to all radii around the origin.
+   * @param constraint The constraint to enforce when the robot is within the
+   * region.
+   */
+  EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
+                             units::meter_t yWidth, const Rotation2d& rotation,
+                             const Constraint& constraint)
+      : m_center(center),
+        m_radii(xWidth / 2.0, yWidth / 2.0),
+        m_constraint(constraint) {
+    m_radii = m_radii.RotateBy(rotation);
+  }
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MaxVelocity(pose, curvature, velocity);
+    } else {
+      return units::meters_per_second_t(
+          std::numeric_limits<double>::infinity());
+    }
+  }
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+    } else {
+      return {};
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the
+   * constraint is enforced in.
+   *
+   * @param pose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  bool IsPoseInRegion(const Pose2d& pose) const {
+    // The region (disk) bounded by the ellipse is given by the equation:
+    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+    // If the inequality is satisfied, then it is inside the ellipse; otherwise
+    // it is outside the ellipse.
+    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
+    return units::math::pow<2>(pose.X() - m_center.X()) *
+                   units::math::pow<2>(m_radii.Y()) +
+               units::math::pow<2>(pose.Y() - m_center.Y()) *
+                   units::math::pow<2>(m_radii.X()) <=
+           units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
+  }
+
+ private:
+  Translation2d m_center;
+  Translation2d m_radii;
+  Constraint m_constraint;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
new file mode 100644
index 0000000..7a30881
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/math.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * Represents a constraint that enforces a max velocity. This can be composed
+ * with the EllipticalRegionConstraint or RectangularRegionConstraint to enforce
+ * a max velocity within a region.
+ */
+class MaxVelocityConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Constructs a new MaxVelocityConstraint.
+   *
+   * @param maxVelocity The max velocity.
+   */
+  explicit MaxVelocityConstraint(units::meters_per_second_t maxVelocity)
+      : m_maxVelocity(units::math::abs(maxVelocity)) {}
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override {
+    return m_maxVelocity;
+  }
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override {
+    return {};
+  }
+
+ private:
+  units::meters_per_second_t m_maxVelocity;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
new file mode 100644
index 0000000..0166f56
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * A class that enforces constraints on the mecanum drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for wheels of the drivetrain stay below a certain
+ * limit.
+ */
+class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  MecanumDriveKinematicsConstraint(const MecanumDriveKinematics& kinematics,
+                                   units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override;
+
+ private:
+  const MecanumDriveKinematics& m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
new file mode 100644
index 0000000..203b237
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <limits>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Enforces a particular constraint only within a rectangular region.
+ */
+template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
+                                   TrajectoryConstraint, Constraint>>>
+class RectangularRegionConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Constructs a new RectangularRegionConstraint.
+   *
+   * @param bottomLeftPoint The bottom left point of the rectangular region in
+   * which to enforce the constraint.
+   * @param topRightPoint The top right point of the rectangular region in which
+   * to enforce the constraint.
+   * @param constraint The constraint to enforce when the robot is within the
+   * region.
+   */
+  RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
+                              const Translation2d& topRightPoint,
+                              const Constraint& constraint)
+      : m_bottomLeftPoint(bottomLeftPoint),
+        m_topRightPoint(topRightPoint),
+        m_constraint(constraint) {}
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MaxVelocity(pose, curvature, velocity);
+    } else {
+      return units::meters_per_second_t(
+          std::numeric_limits<double>::infinity());
+    }
+  }
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override {
+    if (IsPoseInRegion(pose)) {
+      return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+    } else {
+      return {};
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the
+   * constraint is enforced in.
+   *
+   * @param pose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  bool IsPoseInRegion(const Pose2d& pose) const {
+    return pose.X() >= m_bottomLeftPoint.X() &&
+           pose.X() <= m_topRightPoint.X() &&
+           pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y();
+  }
+
+ private:
+  Translation2d m_bottomLeftPoint;
+  Translation2d m_topRightPoint;
+  Constraint m_constraint;
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
new file mode 100644
index 0000000..0f43e29
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+template <size_t NumModules>
+
+/**
+ * A class that enforces constraints on the swerve drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities of the wheels stay below a certain limit.
+ */
+class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  SwerveDriveKinematicsConstraint(
+      const frc::SwerveDriveKinematics<NumModules>& kinematics,
+      units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) const override;
+
+ private:
+  const frc::SwerveDriveKinematics<NumModules>& m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc
+
+#include "SwerveDriveKinematicsConstraint.inc"

wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
new file mode 100644
index 0000000..1af8511
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/math.h"
+
+namespace frc {
+
+template <size_t NumModules>
+
+/**
+ * A class that enforces constraints on the swerve drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities of the wheels stay below a certain limit.
+ */
+SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
+    const frc::SwerveDriveKinematics<NumModules>& kinematics,
+    units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+template <size_t NumModules>
+units::meters_per_second_t
+SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  auto xVelocity = velocity * pose.Rotation().Cos();
+  auto yVelocity = velocity * pose.Rotation().Sin();
+  auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
+      {xVelocity, yVelocity, velocity * curvature});
+  m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
+
+  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+template <size_t NumModules>
+TrajectoryConstraint::MinMax
+SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}
+
+}  // namespace frc

wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h

diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
new file mode 100644
index 0000000..b5548c5
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <limits>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/spline/Spline.h"
+#include "units/acceleration.h"
+#include "units/curvature.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * An interface for defining user-defined velocity and acceleration constraints
+ * while generating trajectories.
+ */
+class TrajectoryConstraint {
+ public:
+  TrajectoryConstraint() = default;
+
+  TrajectoryConstraint(const TrajectoryConstraint&) = default;
+  TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default;
+
+  TrajectoryConstraint(TrajectoryConstraint&&) = default;
+  TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default;
+
+  virtual ~TrajectoryConstraint() = default;
+
+  /**
+   * Represents a minimum and maximum acceleration.
+   */
+  struct MinMax {
+    /**
+     * The minimum acceleration.
+     */
+    units::meters_per_second_squared_t minAcceleration{
+        -std::numeric_limits<double>::max()};
+
+    /**
+     * The maximum acceleration.
+     */
+    units::meters_per_second_squared_t maxAcceleration{
+        std::numeric_limits<double>::max()};
+  };
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param pose The pose at the current point in the trajectory.
+   * @param curvature The curvature at the current point in the trajectory.
+   * @param velocity The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   *
+   * @return The absolute maximum velocity.
+   */
+  virtual units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) const = 0;
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param pose The pose at the current point in the trajectory.
+   * @param curvature The curvature at the current point in the trajectory.
+   * @param speed The speed at the current point in the trajectory.
+   *
+   * @return The min and max acceleration bounds.
+   */
+  virtual MinMax MinMaxAcceleration(const Pose2d& pose,
+                                    units::curvature_t curvature,
+                                    units::meters_per_second_t speed) const = 0;
+};
+}  // namespace frc

wpimath/src/main/native/include/units/acceleration.h

diff --git a/wpimath/src/main/native/include/units/acceleration.h b/wpimath/src/main/native/include/units/acceleration.h
new file mode 100644
index 0000000..5427160
--- /dev/null
+++ b/wpimath/src/main/native/include/units/acceleration.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/length.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::acceleration
+ * @brief namespace for unit types and containers representing acceleration
+ *        values
+ * @details The SI unit for acceleration is `meters_per_second_squared`, and the
+ *          corresponding `base_unit` category is `acceleration_unit`.
+ * @anchor accelerationContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_ACCELERATION_UNITS)
+UNIT_ADD(acceleration, meters_per_second_squared, meters_per_second_squared,
+         mps_sq, unit<std::ratio<1>, units::category::acceleration_unit>)
+UNIT_ADD(acceleration, feet_per_second_squared, feet_per_second_squared, fps_sq,
+         compound_unit<length::feet, inverse<squared<time::seconds>>>)
+UNIT_ADD(acceleration, standard_gravity, standard_gravity, SG,
+         unit<std::ratio<980665, 100000>, meters_per_second_squared>)
+
+UNIT_ADD_CATEGORY_TRAIT(acceleration)
+#endif
+
+using namespace acceleration;
+}  // namespace units

wpimath/src/main/native/include/units/angle.h

diff --git a/wpimath/src/main/native/include/units/angle.h b/wpimath/src/main/native/include/units/angle.h
new file mode 100644
index 0000000..a0f802f
--- /dev/null
+++ b/wpimath/src/main/native/include/units/angle.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::angle
+ * @brief namespace for unit types and containers representing angle values
+ * @details The SI unit for angle is `radians`, and the corresponding
+ *          `base_unit` category is`angle_unit`.
+ * @anchor angleContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(angle, radian, radians, rad,
+                              unit<std::ratio<1>, units::category::angle_unit>)
+UNIT_ADD(angle, degree, degrees, deg,
+         unit<std::ratio<1, 180>, radians, std::ratio<1>>)
+UNIT_ADD(angle, arcminute, arcminutes, arcmin, unit<std::ratio<1, 60>, degrees>)
+UNIT_ADD(angle, arcsecond, arcseconds, arcsec,
+         unit<std::ratio<1, 60>, arcminutes>)
+UNIT_ADD(angle, milliarcsecond, milliarcseconds, mas, milli<arcseconds>)
+UNIT_ADD(angle, turn, turns, tr, unit<std::ratio<2>, radians, std::ratio<1>>)
+UNIT_ADD(angle, gradian, gradians, gon, unit<std::ratio<1, 400>, turns>)
+
+UNIT_ADD_CATEGORY_TRAIT(angle)
+#endif
+
+using namespace angle;
+}  // namespace units

wpimath/src/main/native/include/units/angular_acceleration.h

diff --git a/wpimath/src/main/native/include/units/angular_acceleration.h b/wpimath/src/main/native/include/units/angular_acceleration.h
new file mode 100644
index 0000000..4b1af0f
--- /dev/null
+++ b/wpimath/src/main/native/include/units/angular_acceleration.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/angular_velocity.h"
+#include "units/base.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::angular_acceleration
+ * @brief namespace for unit types and containers representing angular
+ *        acceleration values
+ * @details The SI unit for angular acceleration is
+ *          `radians_per_second_squared`, and the corresponding `base_unit`
+ *          category is`angular_acceleration_unit`.
+ * @anchor angularAccelerationContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+UNIT_ADD(angular_acceleration, radians_per_second_squared,
+         radians_per_second_squared, rad_per_s_sq,
+         unit<std::ratio<1>, units::category::angular_acceleration_unit>)
+UNIT_ADD(angular_acceleration, degrees_per_second_squared,
+         degrees_per_second_squared, deg_per_s_sq,
+         compound_unit<angle::degrees, inverse<squared<time::seconds>>>)
+
+UNIT_ADD_CATEGORY_TRAIT(angular_acceleration)
+
+using namespace angular_acceleration;
+}  // namespace units

wpimath/src/main/native/include/units/angular_velocity.h

diff --git a/wpimath/src/main/native/include/units/angular_velocity.h b/wpimath/src/main/native/include/units/angular_velocity.h
new file mode 100644
index 0000000..580d021
--- /dev/null
+++ b/wpimath/src/main/native/include/units/angular_velocity.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/angle.h"
+#include "units/base.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::angular_velocity
+ * @brief namespace for unit types and containers representing angular velocity
+ *        values
+ * @details The SI unit for angular velocity is `radians_per_second`, and the
+ *          corresponding `base_unit` category is`angular_velocity_unit`.
+ * @anchor angularVelocityContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_ANGULAR_VELOCITY_UNITS)
+UNIT_ADD(angular_velocity, radians_per_second, radians_per_second, rad_per_s,
+         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, 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,
+         mas_per_yr, compound_unit<angle::milliarcseconds, inverse<time::year>>)
+
+UNIT_ADD_CATEGORY_TRAIT(angular_velocity)
+#endif
+
+using namespace angular_velocity;
+}  // namespace units

wpimath/src/main/native/include/units/area.h

diff --git a/wpimath/src/main/native/include/units/area.h b/wpimath/src/main/native/include/units/area.h
new file mode 100644
index 0000000..1bdd3e3
--- /dev/null
+++ b/wpimath/src/main/native/include/units/area.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/length.h"
+
+namespace units {
+/**
+ * @namespace units::area
+ * @brief namespace for unit types and containers representing area values
+ * @details The SI unit for area is `square_meters`, and the corresponding
+ *          `base_unit` category is `area_unit`.
+ * @anchor areaContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_AREA_UNITS)
+UNIT_ADD(area, square_meter, square_meters, sq_m,
+         unit<std::ratio<1>, units::category::area_unit>)
+UNIT_ADD(area, square_foot, square_feet, sq_ft, squared<length::feet>)
+UNIT_ADD(area, square_inch, square_inches, sq_in, squared<length::inch>)
+UNIT_ADD(area, square_mile, square_miles, sq_mi, squared<length::miles>)
+UNIT_ADD(area, square_kilometer, square_kilometers, sq_km,
+         squared<length::kilometers>)
+UNIT_ADD(area, hectare, hectares, ha, unit<std::ratio<10000>, square_meters>)
+UNIT_ADD(area, acre, acres, acre, unit<std::ratio<43560>, square_feet>)
+
+UNIT_ADD_CATEGORY_TRAIT(area)
+#endif
+
+using namespace area;
+}  // namespace units

wpimath/src/main/native/include/units/base.h

diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h
new file mode 100644
index 0000000..579ec88
--- /dev/null
+++ b/wpimath/src/main/native/include/units/base.h
@@ -0,0 +1,3367 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+// ATTRIBUTION:
+// Parts of this work have been adapted from:
+// http://stackoverflow.com/questions/35069778/create-comparison-trait-for-template-classes-whose-parameters-are-in-a-different
+// http://stackoverflow.com/questions/28253399/check-traits-for-all-variadic-template-arguments/28253503
+// http://stackoverflow.com/questions/36321295/rational-approximation-of-square-root-of-stdratio-at-compile-time?noredirect=1#comment60266601_36321295
+//
+
+/// @file	units.h
+/// @brief Complete implementation of `units` - a compile-time, header-only,
+///        unit conversion library built on c++14 with no dependencies.
+
+#pragma once
+
+#ifdef _MSC_VER
+#	pragma push_macro("pascal")
+#	undef pascal
+#	if _MSC_VER <= 1800
+#		define _ALLOW_KEYWORD_MACROS
+#		pragma warning(push)
+#		pragma warning(disable : 4520)
+#		pragma push_macro("constexpr")
+#		define constexpr /*constexpr*/
+#		pragma push_macro("noexcept")
+#		define noexcept throw()
+#	endif // _MSC_VER < 1800
+#endif // _MSC_VER
+
+#if !defined(_MSC_VER) || _MSC_VER > 1800
+#   define UNIT_HAS_LITERAL_SUPPORT
+#   define UNIT_HAS_VARIADIC_TEMPLATE_SUPPORT
+#endif
+
+#ifndef UNIT_LIB_DEFAULT_TYPE
+#   define UNIT_LIB_DEFAULT_TYPE double
+#endif
+
+//--------------------
+//	INCLUDES
+//--------------------
+
+#include <chrono>
+#include <ratio>
+#include <type_traits>
+#include <cstdint>
+#include <cmath>
+#include <limits>
+
+#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
+	#include <iostream>
+	#include <string>
+	#include <locale>
+
+	//------------------------------
+	//	STRING FORMATTER
+	//------------------------------
+
+	namespace units
+	{
+		namespace detail
+		{
+			template <typename T> std::string to_string(const T& t)
+			{
+				std::string str{ std::to_string(t) };
+				int offset{ 1 };
+
+				// remove trailing decimal points for integer value units. Locale aware!
+				struct lconv * lc;
+				lc = localeconv();
+				char decimalPoint = *lc->decimal_point;
+				if (str.find_last_not_of('0') == str.find(decimalPoint)) { offset = 0; }
+				str.erase(str.find_last_not_of('0') + offset, std::string::npos);
+				return str;
+			}
+		}
+	}
+#endif
+
+namespace units
+{
+	template<typename T> inline constexpr const char* name(const T&);
+	template<typename T> inline constexpr const char* abbreviation(const T&);
+}
+
+//------------------------------
+//	MACROS
+//------------------------------
+
+/**
+ * @def			UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, definition)
+ * @brief		Helper macro for generating the boiler-plate code generating the tags of a new unit.
+ * @details		The macro generates singular, plural, and abbreviated forms
+ *				of the unit definition (e.g. `meter`, `meters`, and `m`), as aliases for the
+ *				unit tag.
+ * @param		namespaceName namespace in which the new units will be encapsulated.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ * @param		namePlural - plural version of the unit name, e.g. 'meters'
+ * @param		abbreviation - abbreviated unit name, e.g. 'm'
+ * @param		definition - the variadic parameter is used for the definition of the unit
+ *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
+ * @note		a variadic template is used for the definition to allow templates with
+ *				commas to be easily expanded. All the variadic 'arguments' should together
+ *				comprise the unit definition.
+ */
+#define UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, /*definition*/...)\
+	namespace namespaceName\
+	{\
+	/** @name Units (full names plural) */ /** @{ */ typedef __VA_ARGS__ namePlural; /** @} */\
+	/** @name Units (full names singular) */ /** @{ */ typedef namePlural nameSingular; /** @} */\
+	/** @name Units (abbreviated) */ /** @{ */ typedef namePlural abbreviation; /** @} */\
+	}
+
+/**
+ * @def			UNIT_ADD_UNIT_DEFINITION(namespaceName,nameSingular)
+ * @brief		Macro for generating the boiler-plate code for the unit_t type definition.
+ * @details		The macro generates the definition of the unit container types, e.g. `meter_t`
+ * @param		namespaceName namespace in which the new units will be encapsulated.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ */
+#define UNIT_ADD_UNIT_DEFINITION(namespaceName,nameSingular)\
+	namespace namespaceName\
+	{\
+		/** @name Unit Containers */ /** @{ */ typedef unit_t<nameSingular> nameSingular ## _t; /** @} */\
+	}
+
+/**
+ * @def			UNIT_ADD_CUSTOM_TYPE_UNIT_DEFINITION(namespaceName,nameSingular,underlyingType)
+ * @brief		Macro for generating the boiler-plate code for a unit_t type definition with a non-default underlying type.
+ * @details		The macro generates the definition of the unit container types, e.g. `meter_t`
+ * @param		namespaceName namespace in which the new units will be encapsulated.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ * @param		underlyingType the underlying type
+ */
+#define UNIT_ADD_CUSTOM_TYPE_UNIT_DEFINITION(namespaceName,nameSingular, underlyingType)\
+	namespace namespaceName\
+	{\
+	/** @name Unit Containers */ /** @{ */ typedef unit_t<nameSingular,underlyingType> nameSingular ## _t; /** @} */\
+	}
+/**
+ * @def			UNIT_ADD_IO(namespaceName,nameSingular, abbreviation)
+ * @brief		Macro for generating the boiler-plate code needed for I/O for a new unit.
+ * @details		The macro generates the code to insert units into an ostream. It
+ *				prints both the value and abbreviation of the unit when invoked.
+ * @param		namespaceName namespace in which the new units will be encapsulated.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ * @param		abbrev - abbreviated unit name, e.g. 'm'
+ * @note		When UNIT_LIB_DISABLE_IOSTREAM is defined, the macro does not generate any code
+ */
+#if defined(UNIT_LIB_DISABLE_IOSTREAM)
+	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)
+#else
+	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
+	namespace namespaceName\
+	{\
+		inline std::ostream& operator<<(std::ostream& os, const nameSingular ## _t& obj) \
+		{\
+			os << obj() << " "#abbrev; return os; \
+		}\
+		inline std::string to_string(const nameSingular ## _t& obj)\
+		{\
+			return units::detail::to_string(obj()) + std::string(" "#abbrev);\
+		}\
+	}
+#endif
+
+ /**
+  * @def		UNIT_ADD_NAME(namespaceName,nameSingular,abbreviation)
+  * @brief		Macro for generating constexpr names/abbreviations for units.
+  * @details	The macro generates names for units. E.g. name() of 1_m would be "meter", and
+  *				abbreviation would be "m".
+  * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
+  *				are placed in the `units::literals` namespace.
+  * @param		nameSingular singular version of the unit name, e.g. 'meter'
+  * @param		abbreviation - abbreviated unit name, e.g. 'm'
+  */
+#define UNIT_ADD_NAME(namespaceName, nameSingular, abbrev)\
+template<> inline constexpr const char* name(const namespaceName::nameSingular ## _t&)\
+{\
+	return #nameSingular;\
+}\
+template<> inline constexpr const char* abbreviation(const namespaceName::nameSingular ## _t&)\
+{\
+	return #abbrev;\
+}
+
+/**
+ * @def			UNIT_ADD_LITERALS(namespaceName,nameSingular,abbreviation)
+ * @brief		Macro for generating user-defined literals for units.
+ * @details		The macro generates user-defined literals for units. A literal suffix is created
+ *				using the abbreviation (e.g. `10.0_m`).
+ * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
+ *				are placed in the `units::literals` namespace.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ * @param		abbreviation - abbreviated unit name, e.g. 'm'
+ * @note		When UNIT_HAS_LITERAL_SUPPORT is not defined, the macro does not generate any code
+ */
+#if defined(UNIT_HAS_LITERAL_SUPPORT)
+	#define UNIT_ADD_LITERALS(namespaceName, nameSingular, abbreviation)\
+	namespace literals\
+	{\
+		inline constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation(long double d)\
+		{\
+			return namespaceName::nameSingular ## _t(static_cast<namespaceName::nameSingular ## _t::underlying_type>(d));\
+		}\
+		inline constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation (unsigned long long d)\
+		{\
+			return namespaceName::nameSingular ## _t(static_cast<namespaceName::nameSingular ## _t::underlying_type>(d));\
+		}\
+	}
+#else
+	#define UNIT_ADD_LITERALS(namespaceName, nameSingular, abbreviation)
+#endif
+
+/**
+ * @def			UNIT_ADD(namespaceName,nameSingular, namePlural, abbreviation, definition)
+ * @brief		Macro for generating the boiler-plate code needed for a new unit.
+ * @details		The macro generates singular, plural, and abbreviated forms
+ *				of the unit definition (e.g. `meter`, `meters`, and `m`), as well as the
+ *				appropriately named unit container (e.g. `meter_t`). A literal suffix is created
+ *				using the abbreviation (e.g. `10.0_m`). It also defines a class-specific
+ *				cout function which prints both the value and abbreviation of the unit when invoked.
+ * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
+ *				are placed in the `units::literals` namespace.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ * @param		namePlural - plural version of the unit name, e.g. 'meters'
+ * @param		abbreviation - abbreviated unit name, e.g. 'm'
+ * @param		definition - the variadic parameter is used for the definition of the unit
+ *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
+ * @note		a variadic template is used for the definition to allow templates with
+ *				commas to be easily expanded. All the variadic 'arguments' should together
+ *				comprise the unit definition.
+ */
+#define UNIT_ADD(namespaceName, nameSingular, namePlural, abbreviation, /*definition*/...)\
+	UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, __VA_ARGS__)\
+	UNIT_ADD_UNIT_DEFINITION(namespaceName,nameSingular)\
+	UNIT_ADD_NAME(namespaceName,nameSingular, abbreviation)\
+	UNIT_ADD_IO(namespaceName,nameSingular, abbreviation)\
+	UNIT_ADD_LITERALS(namespaceName,nameSingular, abbreviation)
+
+/**
+ * @def			UNIT_ADD_WITH_CUSTOM_TYPE(namespaceName,nameSingular, namePlural, abbreviation, underlyingType, definition)
+ * @brief		Macro for generating the boiler-plate code needed for a new unit with a non-default underlying type.
+ * @details		The macro generates singular, plural, and abbreviated forms
+ *				of the unit definition (e.g. `meter`, `meters`, and `m`), as well as the
+ *				appropriately named unit container (e.g. `meter_t`). A literal suffix is created
+ *				using the abbreviation (e.g. `10.0_m`). It also defines a class-specific
+ *				cout function which prints both the value and abbreviation of the unit when invoked.
+ * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
+ *				are placed in the `units::literals` namespace.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ * @param		namePlural - plural version of the unit name, e.g. 'meters'
+ * @param		abbreviation - abbreviated unit name, e.g. 'm'
+ * @param		underlyingType - the underlying type, e.g. 'int' or 'float'
+ * @param		definition - the variadic parameter is used for the definition of the unit
+ *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
+ * @note		a variadic template is used for the definition to allow templates with
+ *				commas to be easily expanded. All the variadic 'arguments' should together
+ *				comprise the unit definition.
+ */
+#define UNIT_ADD_WITH_CUSTOM_TYPE(namespaceName, nameSingular, namePlural, abbreviation, underlyingType, /*definition*/...)\
+	UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, __VA_ARGS__)\
+	UNIT_ADD_CUSTOM_TYPE_UNIT_DEFINITION(namespaceName,nameSingular,underlyingType)\
+	UNIT_ADD_IO(namespaceName,nameSingular, abbreviation)\
+	UNIT_ADD_LITERALS(namespaceName,nameSingular, abbreviation)
+
+/**
+ * @def			UNIT_ADD_DECIBEL(namespaceName, nameSingular, abbreviation)
+ * @brief		Macro to create decibel container and literals for an existing unit type.
+ * @details		This macro generates the decibel unit container, cout overload, and literal definitions.
+ * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
+ *				are placed in the `units::literals` namespace.
+ * @param		nameSingular singular version of the base unit name, e.g. 'watt'
+ * @param		abbreviation - abbreviated decibel unit name, e.g. 'dBW'
+ */
+#define UNIT_ADD_DECIBEL(namespaceName, nameSingular, abbreviation)\
+	namespace namespaceName\
+	{\
+		/** @name Unit Containers */ /** @{ */ typedef unit_t<nameSingular, UNIT_LIB_DEFAULT_TYPE, units::decibel_scale> abbreviation ## _t; /** @} */\
+	}\
+	UNIT_ADD_IO(namespaceName, abbreviation, abbreviation)\
+	UNIT_ADD_LITERALS(namespaceName, abbreviation, abbreviation)
+
+/**
+ * @def			UNIT_ADD_CATEGORY_TRAIT(unitCategory, baseUnit)
+ * @brief		Macro to create the `is_category_unit` type trait.
+ * @details		This trait allows users to test whether a given type matches
+ *				an intended category. This macro comprises all the boiler-plate
+ *				code necessary to do so.
+ * @param		unitCategory The name of the category of unit, e.g. length or mass.
+ */
+
+#define UNIT_ADD_CATEGORY_TRAIT_DETAIL(unitCategory)\
+	namespace traits\
+	{\
+		/** @cond */\
+		namespace detail\
+		{\
+			template<typename T> struct is_ ## unitCategory ## _unit_impl : std::false_type {};\
+			template<typename C, typename U, typename P, typename T>\
+			struct is_ ## unitCategory ## _unit_impl<units::unit<C, U, P, T>> : std::is_same<units::traits::base_unit_of<typename units::traits::unit_traits<units::unit<C, U, P, T>>::base_unit_type>, units::category::unitCategory ## _unit>::type {};\
+			template<typename U, typename S, template<typename> class N>\
+			struct is_ ## unitCategory ## _unit_impl<units::unit_t<U, S, N>> : std::is_same<units::traits::base_unit_of<typename units::traits::unit_t_traits<units::unit_t<U, S, N>>::unit_type>, units::category::unitCategory ## _unit>::type {};\
+		}\
+		/** @endcond */\
+	}
+
+#if defined(UNIT_HAS_VARIADIC_TEMPLATE_SUPPORT)
+#define UNIT_ADD_IS_UNIT_CATEGORY_TRAIT(unitCategory)\
+	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> {};\
+	}
+#else
+#define UNIT_ADD_IS_UNIT_CATEGORY_TRAIT(unitCategory)\
+	namespace traits\
+	{\
+			template<typename T1, typename T2 = T1, typename T3 = T1>\
+			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>{};\
+	}
+#endif
+
+#define UNIT_ADD_CATEGORY_TRAIT(unitCategory)\
+	UNIT_ADD_CATEGORY_TRAIT_DETAIL(unitCategory)\
+    /** @ingroup	TypeTraits*/\
+	/** @brief		Trait which tests whether a type represents a unit of unitCategory*/\
+	/** @details	Inherits from `std::true_type` or `std::false_type`. Use `is_ ## unitCategory ## _unit<T>::value` to test the unit represents a unitCategory quantity.*/\
+	/** @tparam		T	one or more types to test*/\
+	UNIT_ADD_IS_UNIT_CATEGORY_TRAIT(unitCategory)
+
+/**
+ * @def			UNIT_ADD_WITH_METRIC_PREFIXES(nameSingular, namePlural, abbreviation, definition)
+ * @brief		Macro for generating the boiler-plate code needed for a new unit, including its metric
+ *				prefixes from femto to peta.
+ * @details		See UNIT_ADD. In addition to generating the unit definition and containers '(e.g. `meters` and 'meter_t',
+ *				it also creates corresponding units with metric suffixes such as `millimeters`, and `millimeter_t`), as well as the
+ *				literal suffixes (e.g. `10.0_mm`).
+ * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
+ *				are placed in the `units::literals` namespace.
+ * @param		nameSingular singular version of the unit name, e.g. 'meter'
+ * @param		namePlural - plural version of the unit name, e.g. 'meters'
+ * @param		abbreviation - abbreviated unit name, e.g. 'm'
+ * @param		definition - the variadic parameter is used for the definition of the unit
+ *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
+ * @note		a variadic template is used for the definition to allow templates with
+ *				commas to be easily expanded. All the variadic 'arguments' should together
+ *				comprise the unit definition.
+ */
+#define UNIT_ADD_WITH_METRIC_PREFIXES(namespaceName, nameSingular, namePlural, abbreviation, /*definition*/...)\
+	UNIT_ADD(namespaceName, nameSingular, namePlural, abbreviation, __VA_ARGS__)\
+	UNIT_ADD(namespaceName, femto ## nameSingular, femto ## namePlural, f ## abbreviation, femto<namePlural>)\
+	UNIT_ADD(namespaceName, pico ## nameSingular, pico ## namePlural, p ## abbreviation, pico<namePlural>)\
+	UNIT_ADD(namespaceName, nano ## nameSingular, nano ## namePlural, n ## abbreviation, nano<namePlural>)\
+	UNIT_ADD(namespaceName, micro ## nameSingular, micro ## namePlural, u ## abbreviation, micro<namePlural>)\
+	UNIT_ADD(namespaceName, milli ## nameSingular, milli ## namePlural, m ## abbreviation, milli<namePlural>)\
+	UNIT_ADD(namespaceName, centi ## nameSingular, centi ## namePlural, c ## abbreviation, centi<namePlural>)\
+	UNIT_ADD(namespaceName, deci ## nameSingular, deci ## namePlural, d ## abbreviation, deci<namePlural>)\
+	UNIT_ADD(namespaceName, deca ## nameSingular, deca ## namePlural, da ## abbreviation, deca<namePlural>)\
+	UNIT_ADD(namespaceName, hecto ## nameSingular, hecto ## namePlural, h ## abbreviation, hecto<namePlural>)\
+	UNIT_ADD(namespaceName, kilo ## nameSingular, kilo ## namePlural, k ## abbreviation, kilo<namePlural>)\
+	UNIT_ADD(namespaceName, mega ## nameSingular, mega ## namePlural, M ## abbreviation, mega<namePlural>)\
+	UNIT_ADD(namespaceName, giga ## nameSingular, giga ## namePlural, G ## abbreviation, giga<namePlural>)\
+	UNIT_ADD(namespaceName, tera ## nameSingular, tera ## namePlural, T ## abbreviation, tera<namePlural>)\
+	UNIT_ADD(namespaceName, peta ## nameSingular, peta ## namePlural, P ## abbreviation, peta<namePlural>)\
+
+ /**
+  * @def		UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(nameSingular, namePlural, abbreviation, definition)
+  * @brief		Macro for generating the boiler-plate code needed for a new unit, including its metric
+  *				prefixes from femto to peta, and binary prefixes from kibi to exbi.
+  * @details	See UNIT_ADD. In addition to generating the unit definition and containers '(e.g. `bytes` and 'byte_t',
+  *				it also creates corresponding units with metric suffixes such as `millimeters`, and `millimeter_t`), as well as the
+  *				literal suffixes (e.g. `10.0_B`).
+  * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
+  *				are placed in the `units::literals` namespace.
+  * @param		nameSingular singular version of the unit name, e.g. 'byte'
+  * @param		namePlural - plural version of the unit name, e.g. 'bytes'
+  * @param		abbreviation - abbreviated unit name, e.g. 'B'
+  * @param		definition - the variadic parameter is used for the definition of the unit
+  *				(e.g. `unit<std::ratio<1>, units::category::data_unit>`)
+  * @note		a variadic template is used for the definition to allow templates with
+  *				commas to be easily expanded. All the variadic 'arguments' should together
+  *				comprise the unit definition.
+  */
+#define UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(namespaceName, nameSingular, namePlural, abbreviation, /*definition*/...)\
+	UNIT_ADD_WITH_METRIC_PREFIXES(namespaceName, nameSingular, namePlural, abbreviation, __VA_ARGS__)\
+	UNIT_ADD(namespaceName, kibi ## nameSingular, kibi ## namePlural, Ki ## abbreviation, kibi<namePlural>)\
+	UNIT_ADD(namespaceName, mebi ## nameSingular, mebi ## namePlural, Mi ## abbreviation, mebi<namePlural>)\
+	UNIT_ADD(namespaceName, gibi ## nameSingular, gibi ## namePlural, Gi ## abbreviation, gibi<namePlural>)\
+	UNIT_ADD(namespaceName, tebi ## nameSingular, tebi ## namePlural, Ti ## abbreviation, tebi<namePlural>)\
+	UNIT_ADD(namespaceName, pebi ## nameSingular, pebi ## namePlural, Pi ## abbreviation, pebi<namePlural>)\
+	UNIT_ADD(namespaceName, exbi ## nameSingular, exbi ## namePlural, Ei ## abbreviation, exbi<namePlural>)
+
+//--------------------
+//	UNITS NAMESPACE
+//--------------------
+
+/**
+ * @namespace units
+ * @brief Unit Conversion Library namespace
+ */
+namespace units
+{
+	//----------------------------------
+	//	DOXYGEN
+	//----------------------------------
+
+	/**
+	 * @defgroup	UnitContainers Unit Containers
+	 * @brief		Defines a series of classes which contain dimensioned values. Unit containers
+	 *				store a value, and support various arithmetic operations.
+	 */
+
+	/**
+	 * @defgroup	UnitTypes Unit Types
+	 * @brief		Defines a series of classes which represent units. These types are tags used by
+	 *				the conversion function, to create compound units, or to create `unit_t` types.
+	 *				By themselves, they are not containers and have no stored value.
+	 */
+
+	/**
+	 * @defgroup	UnitManipulators Unit Manipulators
+	 * @brief		Defines a series of classes used to manipulate unit types, such as `inverse<>`, `squared<>`, and metric prefixes.
+	 *				Unit manipulators can be chained together, e.g. `inverse<squared<pico<time::seconds>>>` to
+	 *				represent picoseconds^-2.
+	 */
+
+	 /**
+	  * @defgroup	CompileTimeUnitManipulators Compile-time Unit Manipulators
+	  * @brief		Defines a series of classes used to manipulate `unit_value_t` types at compile-time, such as `unit_value_add<>`, `unit_value_sqrt<>`, etc.
+	  *				Compile-time manipulators can be chained together, e.g. `unit_value_sqrt<unit_value_add<unit_value_power<a, 2>, unit_value_power<b, 2>>>` to
+	  *				represent `c = sqrt(a^2 + b^2).
+	  */
+
+	 /**
+	 * @defgroup	UnitMath Unit Math
+	 * @brief		Defines a collection of unit-enabled, strongly-typed versions of `<cmath>` functions.
+	 * @details		Includes most c++11 extensions.
+	 */
+
+	/**
+	 * @defgroup	Conversion Explicit Conversion
+	 * @brief		Functions used to convert values of one logical type to another.
+	 */
+
+	/**
+	 * @defgroup	TypeTraits Type Traits
+	 * @brief		Defines a series of classes to obtain unit type information at compile-time.
+	 */
+
+	//------------------------------
+	//	FORWARD DECLARATIONS
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace constants
+	{
+		namespace detail
+		{
+			static constexpr const UNIT_LIB_DEFAULT_TYPE PI_VAL = 3.14159265358979323846264338327950288419716939937510;
+		}
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	//------------------------------
+	//	RATIO TRAITS
+	//------------------------------
+
+	/**
+	 * @ingroup TypeTraits
+	 * @{
+	 */
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/// has_num implementation.
+		template<class T>
+		struct has_num_impl
+		{
+			template<class U>
+			static constexpr auto test(U*)->std::is_integral<decltype(U::num)> {return std::is_integral<decltype(U::num)>{}; }
+			template<typename>
+			static constexpr std::false_type test(...) { return std::false_type{}; }
+
+			using type = decltype(test<T>(0));
+		};
+	}
+
+	/**
+	 * @brief		Trait which checks for the existence of a static numerator.
+	 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_num<T>::value` to test
+	 *				whether `class T` has a numerator static member.
+	 */
+	template<class T>
+	struct has_num : units::detail::has_num_impl<T>::type {};
+
+	namespace detail
+	{
+		/// has_den implementation.
+		template<class T>
+		struct has_den_impl
+		{
+			template<class U>
+			static constexpr auto test(U*)->std::is_integral<decltype(U::den)> { return std::is_integral<decltype(U::den)>{}; }
+			template<typename>
+			static constexpr std::false_type test(...) { return std::false_type{}; }
+
+			using type = decltype(test<T>(0));
+		};
+	}
+
+	/**
+	 * @brief		Trait which checks for the existence of a static denominator.
+	 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_den<T>::value` to test
+	 *				whether `class T` has a denominator static member.
+	 */
+	template<class T>
+	struct has_den : units::detail::has_den_impl<T>::type {};
+
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace traits
+	{
+		/**
+		 * @brief		Trait that tests whether a type represents a std::ratio.
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_ratio<T>::value` to test
+		 *				whether `class T` implements a std::ratio.
+		 */
+		template<class T>
+		struct is_ratio : std::integral_constant<bool,
+			has_num<T>::value &&
+			has_den<T>::value>
+		{};
+	}
+
+	//------------------------------
+	//	UNIT TRAITS
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	/**
+	 * @brief		void type.
+	 * @details		Helper class for creating type traits.
+	 */
+	template<class ...>
+	struct void_t { typedef void type; };
+
+	/**
+	 * @brief		parameter pack for boolean arguments.
+	 */
+	template<bool...> struct bool_pack {};
+
+	/**
+	 * @brief		Trait which tests that a set of other traits are all true.
+	 */
+	template<bool... Args>
+	struct all_true : std::is_same<units::bool_pack<true, Args...>, units::bool_pack<Args..., true>> {};
+	/** @endcond */	// DOXYGEN IGNORE
+
+	/**
+	 * @brief namespace representing type traits which can access the properties of types provided by the units library.
+	 */
+	namespace traits
+	{
+#ifdef FOR_DOXYGEN_PURPOSES_ONLY
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Traits class defining the properties of units.
+		 * @details		The units library determines certain properties of the units passed to
+		 *				them and what they represent by using the members of the corresponding
+		 *				unit_traits instantiation.
+		 */
+		template<class T>
+		struct unit_traits
+		{
+			typedef typename T::base_unit_type base_unit_type;											///< Unit type that the unit was derived from. May be a `base_unit` or another `unit`. Use the `base_unit_of` trait to find the SI base unit type. This will be `void` if type `T` is not a unit.
+			typedef typename T::conversion_ratio conversion_ratio;										///< `std::ratio` representing the conversion factor to the `base_unit_type`. This will be `void` if type `T` is not a unit.
+			typedef typename T::pi_exponent_ratio pi_exponent_ratio;									///< `std::ratio` representing the exponent of pi to be used in the conversion. This will be `void` if type `T` is not a unit.
+			typedef typename T::translation_ratio translation_ratio;									///< `std::ratio` representing a datum translation to the base unit (i.e. degrees C to degrees F conversion). This will be `void` if type `T` is not a unit.
+		};
+#endif
+		/** @cond */	// DOXYGEN IGNORE
+		/**
+		 * @brief		unit traits implementation for classes which are not units.
+		 */
+		template<class T, typename = void>
+		struct unit_traits
+		{
+			typedef void base_unit_type;
+			typedef void conversion_ratio;
+			typedef void pi_exponent_ratio;
+			typedef void translation_ratio;
+		};
+
+		template<class T>
+		struct unit_traits
+			<T, typename void_t<
+			typename T::base_unit_type,
+			typename T::conversion_ratio,
+			typename T::pi_exponent_ratio,
+			typename T::translation_ratio>::type>
+		{
+			typedef typename T::base_unit_type base_unit_type;											///< Unit type that the unit was derived from. May be a `base_unit` or another `unit`. Use the `base_unit_of` trait to find the SI base unit type. This will be `void` if type `T` is not a unit.
+			typedef typename T::conversion_ratio conversion_ratio;										///< `std::ratio` representing the conversion factor to the `base_unit_type`. This will be `void` if type `T` is not a unit.
+			typedef typename T::pi_exponent_ratio pi_exponent_ratio;									///< `std::ratio` representing the exponent of pi to be used in the conversion. This will be `void` if type `T` is not a unit.
+			typedef typename T::translation_ratio translation_ratio;									///< `std::ratio` representing a datum translation to the base unit (i.e. degrees C to degrees F conversion). This will be `void` if type `T` is not a unit.
+		};
+		/** @endcond */	// END DOXYGEN IGNORE
+	}
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		helper type to identify base units.
+		 * @details		A non-templated base class for `base_unit` which enables RTTI testing.
+		 */
+		struct _base_unit_t {};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace traits
+	{
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests if a class is a `base_unit` type.
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_base_unit<T>::value` to test
+		 *				whether `class T` implements a `base_unit`.
+		 */
+		template<class T>
+		struct is_base_unit : std::is_base_of<units::detail::_base_unit_t, T> {};
+	}
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		helper type to identify units.
+		 * @details		A non-templated base class for `unit` which enables RTTI testing.
+		 */
+		struct _unit {};
+
+		template<std::intmax_t Num, std::intmax_t Den = 1>
+		using meter_ratio = std::ratio<Num, Den>;
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace traits
+	{
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Traits which tests if a class is a `unit`
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_unit<T>::value` to test
+		 *				whether `class T` implements a `unit`.
+		 */
+		template<class T>
+		struct is_unit : std::is_base_of<units::detail::_unit, T>::type {};
+	}
+
+	/** @} */ // end of TypeTraits
+
+	//------------------------------
+	//	BASE UNIT CLASS
+	//------------------------------
+
+	/**
+	 * @ingroup		UnitTypes
+	 * @brief		Class representing SI base unit types.
+	 * @details		Base units are represented by a combination of `std::ratio` template parameters, each
+	 *				describing the exponent of the type of unit they represent. Example: meters per second
+	 *				would be described by a +1 exponent for meters, and a -1 exponent for seconds, thus:
+	 *				`base_unit<std::ratio<1>, std::ratio<0>, std::ratio<-1>>`
+	 * @tparam		Meter		`std::ratio` representing the exponent value for meters.
+	 * @tparam		Kilogram	`std::ratio` representing the exponent value for kilograms.
+	 * @tparam		Second		`std::ratio` representing the exponent value for seconds.
+	 * @tparam		Radian		`std::ratio` representing the exponent value for radians. Although radians are not SI base units, they are included because radians are described by the SI as m * m^-1, which would make them indistinguishable from scalars.
+	 * @tparam		Ampere		`std::ratio` representing the exponent value for amperes.
+	 * @tparam		Kelvin		`std::ratio` representing the exponent value for Kelvin.
+	 * @tparam		Mole		`std::ratio` representing the exponent value for moles.
+	 * @tparam		Candela		`std::ratio` representing the exponent value for candelas.
+	 * @tparam		Byte		`std::ratio` representing the exponent value for bytes.
+	 * @sa			category	 for type aliases for SI base_unit types.
+	 */
+	template<class Meter = detail::meter_ratio<0>,
+	class Kilogram = std::ratio<0>,
+	class Second = std::ratio<0>,
+	class Radian = std::ratio<0>,
+	class Ampere = std::ratio<0>,
+	class Kelvin = std::ratio<0>,
+	class Mole = std::ratio<0>,
+	class Candela = std::ratio<0>,
+	class Byte = std::ratio<0>>
+	struct base_unit : units::detail::_base_unit_t
+	{
+		static_assert(traits::is_ratio<Meter>::value, "Template parameter `Meter` must be a `std::ratio` representing the exponent of meters the unit has");
+		static_assert(traits::is_ratio<Kilogram>::value, "Template parameter `Kilogram` must be a `std::ratio` representing the exponent of kilograms the unit has");
+		static_assert(traits::is_ratio<Second>::value, "Template parameter `Second` must be a `std::ratio` representing the exponent of seconds the unit has");
+		static_assert(traits::is_ratio<Ampere>::value, "Template parameter `Ampere` must be a `std::ratio` representing the exponent of amperes the unit has");
+		static_assert(traits::is_ratio<Kelvin>::value, "Template parameter `Kelvin` must be a `std::ratio` representing the exponent of kelvin the unit has");
+		static_assert(traits::is_ratio<Candela>::value, "Template parameter `Candela` must be a `std::ratio` representing the exponent of candelas the unit has");
+		static_assert(traits::is_ratio<Mole>::value, "Template parameter `Mole` must be a `std::ratio` representing the exponent of moles the unit has");
+		static_assert(traits::is_ratio<Radian>::value, "Template parameter `Radian` must be a `std::ratio` representing the exponent of radians the unit has");
+		static_assert(traits::is_ratio<Byte>::value, "Template parameter `Byte` must be a `std::ratio` representing the exponent of bytes the unit has");
+
+		typedef Meter meter_ratio;
+		typedef Kilogram kilogram_ratio;
+		typedef Second second_ratio;
+		typedef Radian radian_ratio;
+		typedef Ampere ampere_ratio;
+		typedef Kelvin kelvin_ratio;
+		typedef Mole mole_ratio;
+		typedef Candela candela_ratio;
+		typedef Byte byte_ratio;
+	};
+
+	//------------------------------
+	//	UNIT CATEGORIES
+	//------------------------------
+
+	/**
+	 * @brief		namespace representing the implemented base and derived unit types. These will not generally be needed by library users.
+	 * @sa			base_unit for the definition of the category parameters.
+	 */
+	namespace category
+	{
+		// SCALAR (DIMENSIONLESS) TYPES
+		typedef base_unit<> scalar_unit;			///< Represents a quantity with no dimension.
+		typedef base_unit<> dimensionless_unit;	///< Represents a quantity with no dimension.
+
+		// SI BASE UNIT TYPES
+		//					METERS			KILOGRAMS		SECONDS			RADIANS			AMPERES			KELVIN			MOLE			CANDELA			BYTE		---		CATEGORY
+		typedef base_unit<detail::meter_ratio<1>>																																		length_unit;			 		///< Represents an SI base unit of length
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<1>>																														mass_unit;				 		///< Represents an SI base unit of mass
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<1>>																										time_unit;				 		///< Represents an SI base unit of time
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>																						angle_unit;				 		///< Represents an SI base unit of angle
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>																		current_unit;			 		///< Represents an SI base unit of current
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>														temperature_unit;		 		///< Represents an SI base unit of temperature
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>										substance_unit;			 		///< Represents an SI base unit of amount of substance
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>						luminous_intensity_unit; 		///< Represents an SI base unit of luminous intensity
+
+		// SI DERIVED UNIT TYPES
+		//					METERS			KILOGRAMS		SECONDS			RADIANS			AMPERES			KELVIN			MOLE			CANDELA			BYTE		---		CATEGORY
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>>						solid_angle_unit;				///< Represents an SI derived unit of solid angle
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>>																										frequency_unit;					///< Represents an SI derived unit of frequency
+		typedef base_unit<detail::meter_ratio<1>,	std::ratio<0>,	std::ratio<-1>>																										velocity_unit;					///< Represents an SI derived unit of velocity
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>,	std::ratio<1>>																						angular_velocity_unit;			///< Represents an SI derived unit of angular velocity
+		typedef base_unit<detail::meter_ratio<1>,	std::ratio<0>,	std::ratio<-2>>																										acceleration_unit;				///< Represents an SI derived unit of acceleration
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-2>,	std::ratio<1>>																						angular_acceleration_unit;			///< Represents an SI derived unit of angular acceleration
+		typedef base_unit<detail::meter_ratio<1>,	std::ratio<1>,	std::ratio<-2>>																										force_unit;						///< Represents an SI derived unit of force
+		typedef base_unit<detail::meter_ratio<-1>,	std::ratio<1>,	std::ratio<-2>>																										pressure_unit;					///< Represents an SI derived unit of pressure
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<1>,	std::ratio<0>,	std::ratio<1>>																		charge_unit;					///< Represents an SI derived unit of charge
+		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>>																										energy_unit;					///< Represents an SI derived unit of energy
+		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-3>>																										power_unit;						///< Represents an SI derived unit of power
+		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-3>,	std::ratio<0>,	std::ratio<-1>>																		voltage_unit;					///< Represents an SI derived unit of voltage
+		typedef base_unit<detail::meter_ratio<-2>,	std::ratio<-1>,	std::ratio<4>,	std::ratio<0>,	std::ratio<2>>																		capacitance_unit;				///< Represents an SI derived unit of capacitance
+		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-3>,	std::ratio<0>,	std::ratio<-2>>																		impedance_unit;					///< Represents an SI derived unit of impedance
+		typedef base_unit<detail::meter_ratio<-2>,	std::ratio<-1>,	std::ratio<3>,	std::ratio<0>,	std::ratio<2>>																		conductance_unit;				///< Represents an SI derived unit of conductance
+		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>,	std::ratio<0>,	std::ratio<-1>>																		magnetic_flux_unit;				///< Represents an SI derived unit of magnetic flux
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<1>,	std::ratio<-2>,	std::ratio<0>,	std::ratio<-1>>																		magnetic_field_strength_unit;	///< Represents an SI derived unit of magnetic field strength
+		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>,	std::ratio<0>,	std::ratio<-2>>																		inductance_unit;				///< Represents an SI derived unit of inductance
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>						luminous_flux_unit;				///< Represents an SI derived unit of luminous flux
+		typedef base_unit<detail::meter_ratio<-2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>						illuminance_unit;				///< Represents an SI derived unit of illuminance
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>>																										radioactivity_unit;				///< Represents an SI derived unit of radioactivity
+
+		// OTHER UNIT TYPES
+		//					METERS			KILOGRAMS		SECONDS			RADIANS			AMPERES			KELVIN			MOLE			CANDELA			BYTE		---		CATEGORY
+		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>>																										torque_unit;					///< Represents an SI derived unit of torque
+		typedef base_unit<detail::meter_ratio<2>>																																		area_unit;						///< Represents an SI derived unit of area
+		typedef base_unit<detail::meter_ratio<3>>																																		volume_unit;					///< Represents an SI derived unit of volume
+		typedef base_unit<detail::meter_ratio<-3>,	std::ratio<1>>																														density_unit;					///< Represents an SI derived unit of density
+		typedef base_unit<>																																						concentration_unit;				///< Represents a unit of concentration
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>		data_unit;						///< Represents a unit of data size
+		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>		data_transfer_rate_unit;		///< Represents a unit of data transfer rate
+	}
+
+	//------------------------------
+	//	UNIT CLASSES
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	/**
+	 * @brief		unit type template specialization for units derived from base units.
+	 */
+	template <class, class, class, class> struct unit;
+	template<class Conversion, class... Exponents, class PiExponent, class Translation>
+	struct unit<Conversion, base_unit<Exponents...>, PiExponent, Translation> : units::detail::_unit
+	{
+		static_assert(traits::is_ratio<Conversion>::value, "Template parameter `Conversion` must be a `std::ratio` representing the conversion factor to `BaseUnit`.");
+		static_assert(traits::is_ratio<PiExponent>::value, "Template parameter `PiExponent` must be a `std::ratio` representing the exponents of Pi the unit has.");
+		static_assert(traits::is_ratio<Translation>::value, "Template parameter `Translation` must be a `std::ratio` representing an additive translation required by the unit conversion.");
+
+		typedef typename units::base_unit<Exponents...> base_unit_type;
+		typedef Conversion conversion_ratio;
+		typedef Translation translation_ratio;
+		typedef PiExponent pi_exponent_ratio;
+	};
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @brief		Type representing an arbitrary unit.
+	 * @ingroup		UnitTypes
+	 * @details		`unit` types are used as tags for the `conversion` function. They are *not* containers
+	 *				(see `unit_t` for a  container class). Each unit is defined by:
+	 *
+	 *				- A `std::ratio` defining the conversion factor to the base unit type. (e.g. `std::ratio<1,12>` for inches to feet)
+	 *				- A base unit that the unit is derived from (or a unit category. Must be of type `unit` or `base_unit`)
+	 *				- An exponent representing factors of PI required by the conversion. (e.g. `std::ratio<-1>` for a radians to degrees conversion)
+	 *				- a ratio representing a datum translation required for the conversion (e.g. `std::ratio<32>` for a farenheit to celsius conversion)
+	 *
+	 *				Typically, a specific unit, like `meters`, would be implemented as a type alias
+	 *				of `unit`, i.e. `using meters = unit<std::ratio<1>, units::category::length_unit`, or
+	 *				`using inches = unit<std::ratio<1,12>, feet>`.
+	 * @tparam		Conversion	std::ratio representing scalar multiplication factor.
+	 * @tparam		BaseUnit	Unit type which this unit is derived from. May be a `base_unit`, or another `unit`.
+	 * @tparam		PiExponent	std::ratio representing the exponent of pi required by the conversion.
+	 * @tparam		Translation	std::ratio representing any datum translation required by the conversion.
+	 */
+	template<class Conversion, class BaseUnit, class PiExponent = std::ratio<0>, class Translation = std::ratio<0>>
+	struct unit : units::detail::_unit
+	{
+		static_assert(traits::is_unit<BaseUnit>::value, "Template parameter `BaseUnit` must be a `unit` type.");
+		static_assert(traits::is_ratio<Conversion>::value, "Template parameter `Conversion` must be a `std::ratio` representing the conversion factor to `BaseUnit`.");
+		static_assert(traits::is_ratio<PiExponent>::value, "Template parameter `PiExponent` must be a `std::ratio` representing the exponents of Pi the unit has.");
+
+		typedef typename units::traits::unit_traits<BaseUnit>::base_unit_type base_unit_type;
+		typedef typename std::ratio_multiply<typename BaseUnit::conversion_ratio, Conversion> conversion_ratio;
+		typedef typename std::ratio_add<typename BaseUnit::pi_exponent_ratio, PiExponent> pi_exponent_ratio;
+		typedef typename std::ratio_add<std::ratio_multiply<typename BaseUnit::conversion_ratio, Translation>, typename BaseUnit::translation_ratio> translation_ratio;
+	};
+
+	//------------------------------
+	//	BASE UNIT MANIPULATORS
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		base_unit_of trait implementation
+		 * @details		recursively seeks base_unit type that a unit is derived from. Since units can be
+		 *				derived from other units, the `base_unit_type` typedef may not represent this value.
+		 */
+		template<class> struct base_unit_of_impl;
+		template<class Conversion, class BaseUnit, class PiExponent, class Translation>
+		struct base_unit_of_impl<unit<Conversion, BaseUnit, PiExponent, Translation>> : base_unit_of_impl<BaseUnit> {};
+		template<class... Exponents>
+		struct base_unit_of_impl<base_unit<Exponents...>>
+		{
+			typedef base_unit<Exponents...> type;
+		};
+		template<>
+		struct base_unit_of_impl<void>
+		{
+			typedef void type;
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace traits
+	{
+		/**
+		 * @brief		Trait which returns the `base_unit` type that a unit is originally derived from.
+		 * @details		Since units can be derived from other `unit` types in addition to `base_unit` types,
+		 *				the `base_unit_type` typedef will not always be a `base_unit` (or unit category).
+		 *				Since compatible
+		 */
+		template<class U>
+		using base_unit_of = typename units::detail::base_unit_of_impl<U>::type;
+	}
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		implementation of base_unit_multiply
+		 * @details		'multiples' (adds exponent ratios of) two base unit types. Base units can be found
+		 *				using `base_unit_of`.
+		 */
+		template<class, class> struct base_unit_multiply_impl;
+		template<class... Exponents1, class... Exponents2>
+		struct base_unit_multiply_impl<base_unit<Exponents1...>, base_unit<Exponents2...>> {
+			using type = base_unit<std::ratio_add<Exponents1, Exponents2>...>;
+		};
+
+		/**
+		 * @brief		represents type of two base units multiplied together
+		 */
+		template<class U1, class U2>
+		using base_unit_multiply = typename base_unit_multiply_impl<U1, U2>::type;
+
+		/**
+		 * @brief		implementation of base_unit_divide
+		 * @details		'dived' (subtracts exponent ratios of) two base unit types. Base units can be found
+		 *				using `base_unit_of`.
+		 */
+		template<class, class> struct base_unit_divide_impl;
+		template<class... Exponents1, class... Exponents2>
+		struct base_unit_divide_impl<base_unit<Exponents1...>, base_unit<Exponents2...>> {
+			using type = base_unit<std::ratio_subtract<Exponents1, Exponents2>...>;
+		};
+
+		/**
+		 * @brief		represents the resulting type of `base_unit` U1 divided by U2.
+		 */
+		template<class U1, class U2>
+		using base_unit_divide = typename base_unit_divide_impl<U1, U2>::type;
+
+		/**
+		 * @brief		implementation of inverse_base
+		 * @details		multiplies all `base_unit` exponent ratios by -1. The resulting type represents
+		 *				the inverse base unit of the given `base_unit` type.
+		 */
+		template<class> struct inverse_base_impl;
+
+		template<class... Exponents>
+		struct inverse_base_impl<base_unit<Exponents...>> {
+			using type = base_unit<std::ratio_multiply<Exponents, std::ratio<-1>>...>;
+		};
+
+		/**
+		 * @brief		represent the inverse type of `class U`
+		 * @details		E.g. if `U` is `length_unit`, then `inverse<U>` will represent `length_unit^-1`.
+		 */
+		template<class U> using inverse_base = typename inverse_base_impl<U>::type;
+
+		/**
+		 * @brief		implementation of `squared_base`
+		 * @details		multiplies all the exponent ratios of the given class by 2. The resulting type is
+		 *				equivalent to the given type squared.
+		 */
+		template<class U> struct squared_base_impl;
+		template<class... Exponents>
+		struct squared_base_impl<base_unit<Exponents...>> {
+			using type = base_unit<std::ratio_multiply<Exponents, std::ratio<2>>...>;
+		};
+
+		/**
+		 * @brief		represents the type of a `base_unit` squared.
+		 * @details		E.g. `squared<length_unit>` will represent `length_unit^2`.
+		 */
+		template<class U> using squared_base = typename squared_base_impl<U>::type;
+
+		/**
+		 * @brief		implementation of `cubed_base`
+		 * @details		multiplies all the exponent ratios of the given class by 3. The resulting type is
+		 *				equivalent to the given type cubed.
+		 */
+		template<class U> struct cubed_base_impl;
+		template<class... Exponents>
+		struct cubed_base_impl<base_unit<Exponents...>> {
+			using type = base_unit<std::ratio_multiply<Exponents, std::ratio<3>>...>;
+		};
+
+		/**
+		 * @brief		represents the type of a `base_unit` cubed.
+		 * @details		E.g. `cubed<length_unit>` will represent `length_unit^3`.
+		 */
+		template<class U> using cubed_base = typename cubed_base_impl<U>::type;
+
+		/**
+		 * @brief		implementation of `sqrt_base`
+		 * @details		divides all the exponent ratios of the given class by 2. The resulting type is
+		 *				equivalent to the square root of the given type.
+		 */
+		template<class U> struct sqrt_base_impl;
+		template<class... Exponents>
+		struct sqrt_base_impl<base_unit<Exponents...>> {
+			using type = base_unit<std::ratio_divide<Exponents, std::ratio<2>>...>;
+		};
+
+		/**
+		 * @brief		represents the square-root type of a `base_unit`.
+		 * @details		E.g. `sqrt<length_unit>` will represent `length_unit^(1/2)`.
+		 */
+		template<class U> using sqrt_base = typename sqrt_base_impl<U>::type;
+
+		/**
+		 * @brief		implementation of `cbrt_base`
+		 * @details		divides all the exponent ratios of the given class by 3. The resulting type is
+		 *				equivalent to the given type's cube-root.
+		 */
+		template<class U> struct cbrt_base_impl;
+		template<class... Exponents>
+		struct cbrt_base_impl<base_unit<Exponents...>> {
+			using type = base_unit<std::ratio_divide<Exponents, std::ratio<3>>...>;
+		};
+
+		/**
+		 * @brief		represents the cube-root type of a `base_unit` .
+		 * @details		E.g. `cbrt<length_unit>` will represent `length_unit^(1/3)`.
+		 */
+		template<class U> using cbrt_base = typename cbrt_base_impl<U>::type;
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	//------------------------------
+	//	UNIT MANIPULATORS
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		implementation of `unit_multiply`.
+		 * @details		multiplies two units. The base unit becomes the base units of each with their exponents
+		 *				added together. The conversion factors of each are multiplied by each other. Pi exponent ratios
+		 *				are added, and datum translations are removed.
+		 */
+		template<class Unit1, class Unit2>
+		struct unit_multiply_impl
+		{
+			using type = unit < std::ratio_multiply<typename Unit1::conversion_ratio, typename Unit2::conversion_ratio>,
+				base_unit_multiply <traits::base_unit_of<typename Unit1::base_unit_type>, traits::base_unit_of<typename Unit2::base_unit_type>>,
+				std::ratio_add<typename Unit1::pi_exponent_ratio, typename Unit2::pi_exponent_ratio>,
+				std::ratio < 0 >> ;
+		};
+
+		/**
+		 * @brief		represents the type of two units multiplied together.
+		 * @details		recalculates conversion and exponent ratios at compile-time.
+		 */
+		template<class U1, class U2>
+		using unit_multiply = typename unit_multiply_impl<U1, U2>::type;
+
+		/**
+		 * @brief		implementation of `unit_divide`.
+		 * @details		divides two units. The base unit becomes the base units of each with their exponents
+		 *				subtracted from each other. The conversion factors of each are divided by each other. Pi exponent ratios
+		 *				are subtracted, and datum translations are removed.
+		 */
+		template<class Unit1, class Unit2>
+		struct unit_divide_impl
+		{
+			using type = unit < std::ratio_divide<typename Unit1::conversion_ratio, typename Unit2::conversion_ratio>,
+				base_unit_divide<traits::base_unit_of<typename Unit1::base_unit_type>, traits::base_unit_of<typename Unit2::base_unit_type>>,
+				std::ratio_subtract<typename Unit1::pi_exponent_ratio, typename Unit2::pi_exponent_ratio>,
+				std::ratio < 0 >> ;
+		};
+
+		/**
+		 * @brief		represents the type of two units divided by each other.
+		 * @details		recalculates conversion and exponent ratios at compile-time.
+		 */
+		template<class U1, class U2>
+		using unit_divide = typename unit_divide_impl<U1, U2>::type;
+
+		/**
+		 * @brief		implementation of `inverse`
+		 * @details		inverts a unit (equivalent to 1/unit). The `base_unit` and pi exponents are all multiplied by
+		 *				-1. The conversion ratio numerator and denominator are swapped. Datum translation
+		 *				ratios are removed.
+		 */
+		template<class Unit>
+		struct inverse_impl
+		{
+			using type = unit < std::ratio<Unit::conversion_ratio::den, Unit::conversion_ratio::num>,
+				inverse_base<traits::base_unit_of<typename units::traits::unit_traits<Unit>::base_unit_type>>,
+				std::ratio_multiply<typename units::traits::unit_traits<Unit>::pi_exponent_ratio, std::ratio<-1>>,
+				std::ratio < 0 >> ;	// inverses are rates or change, the translation factor goes away.
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @brief		represents the inverse unit type of `class U`.
+	 * @ingroup		UnitManipulators
+	 * @tparam		U	`unit` type to invert.
+	 * @details		E.g. `inverse<meters>` will represent meters^-1 (i.e. 1/meters).
+	 */
+	template<class U> using inverse = typename units::detail::inverse_impl<U>::type;
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		implementation of `squared`
+		 * @details		Squares the conversion ratio, `base_unit` exponents, pi exponents, and removes
+		 *				datum translation ratios.
+		 */
+		template<class Unit>
+		struct squared_impl
+		{
+			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
+			using Conversion = typename Unit::conversion_ratio;
+			using type = unit < std::ratio_multiply<Conversion, Conversion>,
+				squared_base<traits::base_unit_of<typename Unit::base_unit_type>>,
+				std::ratio_multiply<typename Unit::pi_exponent_ratio, std::ratio<2>>,
+				typename Unit::translation_ratio
+			> ;
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @brief		represents the unit type of `class U` squared
+	 * @ingroup		UnitManipulators
+	 * @tparam		U	`unit` type to square.
+	 * @details		E.g. `square<meters>` will represent meters^2.
+	 */
+	template<class U>
+	using squared = typename units::detail::squared_impl<U>::type;
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+			 * @brief		implementation of `cubed`
+			 * @details		Cubes the conversion ratio, `base_unit` exponents, pi exponents, and removes
+			 *				datum translation ratios.
+			 */
+		template<class Unit>
+		struct cubed_impl
+		{
+			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
+			using Conversion = typename Unit::conversion_ratio;
+			using type = unit < std::ratio_multiply<Conversion, std::ratio_multiply<Conversion, Conversion>>,
+				cubed_base<traits::base_unit_of<typename Unit::base_unit_type>>,
+				std::ratio_multiply<typename Unit::pi_exponent_ratio, std::ratio<3>>,
+				typename Unit::translation_ratio> ;
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @brief		represents the type of `class U` cubed.
+	 * @ingroup		UnitManipulators
+	 * @tparam		U	`unit` type to cube.
+	 * @details		E.g. `cubed<meters>` will represent meters^3.
+	 */
+	template<class U>
+	using cubed = typename units::detail::cubed_impl<U>::type;
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		//----------------------------------
+		//	RATIO_SQRT IMPLEMENTATION
+		//----------------------------------
+
+		using Zero = std::ratio<0>;
+		using One = std::ratio<1>;
+		template <typename R> using Square = std::ratio_multiply<R, R>;
+
+		// Find the largest std::integer N such that Predicate<N>::value is true.
+		template <template <std::intmax_t N> class Predicate, typename enabled = void>
+		struct BinarySearch {
+			template <std::intmax_t N>
+			struct SafeDouble_ {
+				static constexpr const std::intmax_t value = 2 * N;
+				static_assert(value > 0, "Overflows when computing 2 * N");
+			};
+
+			template <intmax_t Lower, intmax_t Upper, typename Condition1 = void, typename Condition2 = void>
+			struct DoubleSidedSearch_ : DoubleSidedSearch_<Lower, Upper,
+				std::integral_constant<bool, (Upper - Lower == 1)>,
+				std::integral_constant<bool, ((Upper - Lower>1 && Predicate<Lower + (Upper - Lower) / 2>::value))>> {};
+
+			template <intmax_t Lower, intmax_t Upper>
+			struct DoubleSidedSearch_<Lower, Upper, std::false_type, std::false_type> : DoubleSidedSearch_<Lower, Lower + (Upper - Lower) / 2> {};
+
+			template <intmax_t Lower, intmax_t Upper, typename Condition2>
+			struct DoubleSidedSearch_<Lower, Upper, std::true_type, Condition2> : std::integral_constant<intmax_t, Lower>{};
+
+			template <intmax_t Lower, intmax_t Upper, typename Condition1>
+			struct DoubleSidedSearch_<Lower, Upper, Condition1, std::true_type> : DoubleSidedSearch_<Lower + (Upper - Lower) / 2, Upper>{};
+
+			template <std::intmax_t Lower, class enabled1 = void>
+			struct SingleSidedSearch_ : SingleSidedSearch_<Lower, std::integral_constant<bool, Predicate<SafeDouble_<Lower>::value>::value>>{};
+
+			template <std::intmax_t Lower>
+			struct SingleSidedSearch_<Lower, std::false_type> : DoubleSidedSearch_<Lower, SafeDouble_<Lower>::value> {};
+
+			template <std::intmax_t Lower>
+			struct SingleSidedSearch_<Lower, std::true_type> : SingleSidedSearch_<SafeDouble_<Lower>::value>{};
+
+			static constexpr const std::intmax_t value = SingleSidedSearch_<1>::value;
+ 		};
+
+		template <template <std::intmax_t N> class Predicate>
+		struct BinarySearch<Predicate, std::enable_if_t<!Predicate<1>::value>> : std::integral_constant<std::intmax_t, 0>{};
+
+		// Find largest std::integer N such that N<=sqrt(R)
+		template <typename R>
+		struct Integer {
+			template <std::intmax_t N> using Predicate_ = std::ratio_less_equal<std::ratio<N>, std::ratio_divide<R, std::ratio<N>>>;
+			static constexpr const std::intmax_t value = BinarySearch<Predicate_>::value;
+		};
+
+		template <typename R>
+		struct IsPerfectSquare {
+			static constexpr const std::intmax_t DenSqrt_ = Integer<std::ratio<R::den>>::value;
+			static constexpr const std::intmax_t NumSqrt_ = Integer<std::ratio<R::num>>::value;
+			static constexpr const bool value =( DenSqrt_ * DenSqrt_ == R::den && NumSqrt_ * NumSqrt_ == R::num);
+			using Sqrt = std::ratio<NumSqrt_, DenSqrt_>;
+		};
+
+		// Represents sqrt(P)-Q.
+		template <typename Tp, typename Tq>
+		struct Remainder {
+			using P = Tp;
+			using Q = Tq;
+		};
+
+		// Represents 1/R = I + Rem where R is a Remainder.
+		template <typename R>
+		struct Reciprocal {
+			using P_ = typename R::P;
+			using Q_ = typename R::Q;
+			using Den_ = std::ratio_subtract<P_, Square<Q_>>;
+			using A_ = std::ratio_divide<Q_, Den_>;
+			using B_ = std::ratio_divide<P_, Square<Den_>>;
+			static constexpr const std::intmax_t I_ = (A_::num + Integer<std::ratio_multiply<B_, Square<std::ratio<A_::den>>>>::value) / A_::den;
+			using I = std::ratio<I_>;
+			using Rem = Remainder<B_, std::ratio_subtract<I, A_>>;
+		};
+
+		// Expands sqrt(R) to continued fraction:
+		// f(x)=C1+1/(C2+1/(C3+1/(...+1/(Cn+x)))) = (U*x+V)/(W*x+1) and sqrt(R)=f(Rem).
+		// The error |f(Rem)-V| = |(U-W*V)x/(W*x+1)| <= |U-W*V|*Rem <= |U-W*V|/I' where
+		// I' is the std::integer part of reciprocal of Rem.
+		template <typename Tr, std::intmax_t N>
+		struct ContinuedFraction {
+			template <typename T>
+			using Abs_ = std::conditional_t<std::ratio_less<T, Zero>::value, std::ratio_subtract<Zero, T>, T>;
+
+			using R = Tr;
+			using Last_ = ContinuedFraction<R, N - 1>;
+			using Reciprocal_ = Reciprocal<typename Last_::Rem>;
+			using Rem = typename Reciprocal_::Rem;
+			using I_ = typename Reciprocal_::I;
+			using Den_ = std::ratio_add<typename Last_::W, I_>;
+			using U = std::ratio_divide<typename Last_::V, Den_>;
+			using V = std::ratio_divide<std::ratio_add<typename Last_::U, std::ratio_multiply<typename Last_::V, I_>>, Den_>;
+			using W = std::ratio_divide<One, Den_>;
+			using Error = Abs_<std::ratio_divide<std::ratio_subtract<U, std::ratio_multiply<V, W>>, typename Reciprocal<Rem>::I>>;
+		};
+
+		template <typename Tr>
+		struct ContinuedFraction<Tr, 1> {
+			using R = Tr;
+			using U = One;
+			using V = std::ratio<Integer<R>::value>;
+			using W = Zero;
+			using Rem = Remainder<R, V>;
+			using Error = std::ratio_divide<One, typename Reciprocal<Rem>::I>;
+		};
+
+		template <typename R, typename Eps, std::intmax_t N = 1, typename enabled = void>
+		struct Sqrt_ : Sqrt_<R, Eps, N + 1> {};
+
+		template <typename R, typename Eps, std::intmax_t N>
+		struct Sqrt_<R, Eps, N, std::enable_if_t<std::ratio_less_equal<typename ContinuedFraction<R, N>::Error, Eps>::value>> {
+			using type = typename ContinuedFraction<R, N>::V;
+		};
+
+		template <typename R, typename Eps, typename enabled = void>
+		struct Sqrt {
+			static_assert(std::ratio_greater_equal<R, Zero>::value, "R can't be negative");
+		};
+
+		template <typename R, typename Eps>
+		struct Sqrt<R, Eps, std::enable_if_t<std::ratio_greater_equal<R, Zero>::value && IsPerfectSquare<R>::value>> {
+			using type = typename IsPerfectSquare<R>::Sqrt;
+		};
+
+		template <typename R, typename Eps>
+		struct Sqrt<R, Eps, std::enable_if_t<(std::ratio_greater_equal<R, Zero>::value && !IsPerfectSquare<R>::value)>> : Sqrt_<R, Eps>{};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @ingroup		TypeTraits
+	 * @brief		Calculate square root of a ratio at compile-time
+	 * @details		Calculates a rational approximation of the square root of the ratio. The error
+	 *				in the calculation is bounded by 1/epsilon (Eps). E.g. for the default value
+	 *				of 10000000000, the maximum error will be a/10000000000, or 1e-8, or said another way,
+	 *				the error will be on the order of 10^-9. Since these calculations are done at
+	 *				compile time, it is advisable to set epsilon to the highest value that does not
+	 *				cause an integer overflow in the calculation. If you can't compile `ratio_sqrt`
+	 *				due to overflow errors, reducing the value of epsilon sufficiently will correct
+	 *				the problem.\n\n
+	 *				`ratio_sqrt` is guaranteed to converge for all values of `Ratio` which do not
+	 *				overflow.
+	 * @note		This function provides a rational approximation, _NOT_ an exact value.
+	 * @tparam		Ratio	ratio to take the square root of. This can represent any rational value,
+	 *						_not_ just integers or values with integer roots.
+	 * @tparam		Eps		Value of epsilon, which represents the inverse of the maximum allowable
+	 *						error. This value should be chosen to be as high as possible before
+	 *						integer overflow errors occur in the compiler.
+	 */
+	template<typename Ratio, std::intmax_t Eps = 10000000000>
+	using ratio_sqrt = typename  units::detail::Sqrt<Ratio, std::ratio<1, Eps>>::type;
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		implementation of `sqrt`
+		 * @details		square roots the conversion ratio, `base_unit` exponents, pi exponents, and removes
+		 *				datum translation ratios.
+		 */
+		template<class Unit, std::intmax_t Eps>
+		struct sqrt_impl
+		{
+			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
+			using Conversion = typename Unit::conversion_ratio;
+			using type = unit <ratio_sqrt<Conversion, Eps>,
+				sqrt_base<traits::base_unit_of<typename Unit::base_unit_type>>,
+				std::ratio_divide<typename Unit::pi_exponent_ratio, std::ratio<2>>,
+				typename Unit::translation_ratio>;
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @ingroup		UnitManipulators
+	 * @brief		represents the square root of type `class U`.
+	 * @details		Calculates a rational approximation of the square root of the unit. The error
+	 *				in the calculation is bounded by 1/epsilon (Eps). E.g. for the default value
+	 *				of 10000000000, the maximum error will be a/10000000000, or 1e-8, or said another way,
+	 *				the error will be on the order of 10^-9. Since these calculations are done at
+	 *				compile time, it is advisable to set epsilon to the highest value that does not
+	 *				cause an integer overflow in the calculation. If you can't compile `ratio_sqrt`
+	 *				due to overflow errors, reducing the value of epsilon sufficiently will correct
+	 *				the problem.\n\n
+	 *				`ratio_sqrt` is guaranteed to converge for all values of `Ratio` which do not
+	 *				overflow.
+	 * @tparam		U	`unit` type to take the square root of.
+	 * @tparam		Eps	Value of epsilon, which represents the inverse of the maximum allowable
+	 *					error. This value should be chosen to be as high as possible before
+	 *					integer overflow errors occur in the compiler.
+	 * @note		USE WITH CAUTION. The is an approximate value. In general, squared<sqrt<meter>> != meter,
+	 *				i.e. the operation is not reversible, and it will result in propogated approximations.
+	 *				Use only when absolutely necessary.
+	 */
+	template<class U, std::intmax_t Eps = 10000000000>
+	using square_root = typename units::detail::sqrt_impl<U, Eps>::type;
+
+	//------------------------------
+	//	COMPOUND UNITS
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+			 * @brief		implementation of compound_unit
+			 * @details		multiplies a variadic list of units together, and is inherited from the resulting
+			 *				type.
+			 */
+		template<class U, class... Us> struct compound_impl;
+		template<class U> struct compound_impl<U> { using type = U; };
+		template<class U1, class U2, class...Us>
+		struct compound_impl<U1, U2, Us...>
+			: compound_impl<unit_multiply<U1, U2>, Us...> {};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @brief		Represents a unit type made up from other units.
+	 * @details		Compound units are formed by multiplying the units of all the types provided in
+	 *				the template argument. Types provided must inherit from `unit`. A compound unit can
+	 *				be formed from any number of other units, and unit manipulators like `inverse` and
+	 *				`squared` are supported. E.g. to specify acceleration, on could create
+	 *				`using acceleration = compound_unit<length::meters, inverse<squared<seconds>>;`
+	 * @tparam		U...	units which, when multiplied together, form the desired compound unit.
+	 * @ingroup		UnitTypes
+	 */
+	template<class U, class... Us>
+	using compound_unit = typename units::detail::compound_impl<U, Us...>::type;
+
+	//------------------------------
+	//	PREFIXES
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/**
+		 * @brief		prefix applicator.
+		 * @details		creates a unit type from a prefix and a unit
+		 */
+		template<class Ratio, class Unit>
+		struct prefix
+		{
+			static_assert(traits::is_ratio<Ratio>::value, "Template parameter `Ratio` must be a `std::ratio`.");
+			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
+			typedef typename units::unit<Ratio, Unit> type;
+		};
+
+		/// recursive exponential implementation
+		template <int N, class U>
+		struct power_of_ratio
+		{
+			typedef std::ratio_multiply<U, typename power_of_ratio<N - 1, U>::type> type;
+		};
+
+		/// End recursion
+		template <class U>
+		struct power_of_ratio<1, U>
+		{
+			typedef U type;
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @ingroup UnitManipulators
+	 * @{
+	 * @ingroup Decimal Prefixes
+	 * @{
+	 */
+	template<class U> using atto	= typename units::detail::prefix<std::atto,	U>::type;			///< Represents the type of `class U` with the metric 'atto' prefix appended.	@details E.g. atto<meters> represents meters*10^-18		@tparam U unit type to apply the prefix to.
+	template<class U> using femto	= typename units::detail::prefix<std::femto,U>::type;			///< Represents the type of `class U` with the metric 'femto' prefix appended.  @details E.g. femto<meters> represents meters*10^-15	@tparam U unit type to apply the prefix to.
+	template<class U> using pico	= typename units::detail::prefix<std::pico,	U>::type;			///< Represents the type of `class U` with the metric 'pico' prefix appended.	@details E.g. pico<meters> represents meters*10^-12		@tparam U unit type to apply the prefix to.
+	template<class U> using nano	= typename units::detail::prefix<std::nano,	U>::type;			///< Represents the type of `class U` with the metric 'nano' prefix appended.	@details E.g. nano<meters> represents meters*10^-9		@tparam U unit type to apply the prefix to.
+	template<class U> using micro	= typename units::detail::prefix<std::micro,U>::type;			///< Represents the type of `class U` with the metric 'micro' prefix appended.	@details E.g. micro<meters> represents meters*10^-6		@tparam U unit type to apply the prefix to.
+	template<class U> using milli	= typename units::detail::prefix<std::milli,U>::type;			///< Represents the type of `class U` with the metric 'milli' prefix appended.	@details E.g. milli<meters> represents meters*10^-3		@tparam U unit type to apply the prefix to.
+	template<class U> using centi	= typename units::detail::prefix<std::centi,U>::type;			///< Represents the type of `class U` with the metric 'centi' prefix appended.	@details E.g. centi<meters> represents meters*10^-2		@tparam U unit type to apply the prefix to.
+	template<class U> using deci	= typename units::detail::prefix<std::deci,	U>::type;			///< Represents the type of `class U` with the metric 'deci' prefix appended.	@details E.g. deci<meters> represents meters*10^-1		@tparam U unit type to apply the prefix to.
+	template<class U> using deca	= typename units::detail::prefix<std::deca,	U>::type;			///< Represents the type of `class U` with the metric 'deca' prefix appended.	@details E.g. deca<meters> represents meters*10^1		@tparam U unit type to apply the prefix to.
+	template<class U> using hecto	= typename units::detail::prefix<std::hecto,U>::type;			///< Represents the type of `class U` with the metric 'hecto' prefix appended.	@details E.g. hecto<meters> represents meters*10^2		@tparam U unit type to apply the prefix to.
+	template<class U> using kilo	= typename units::detail::prefix<std::kilo,	U>::type;			///< Represents the type of `class U` with the metric 'kilo' prefix appended.	@details E.g. kilo<meters> represents meters*10^3		@tparam U unit type to apply the prefix to.
+	template<class U> using mega	= typename units::detail::prefix<std::mega,	U>::type;			///< Represents the type of `class U` with the metric 'mega' prefix appended.	@details E.g. mega<meters> represents meters*10^6		@tparam U unit type to apply the prefix to.
+	template<class U> using giga	= typename units::detail::prefix<std::giga,	U>::type;			///< Represents the type of `class U` with the metric 'giga' prefix appended.	@details E.g. giga<meters> represents meters*10^9		@tparam U unit type to apply the prefix to.
+	template<class U> using tera	= typename units::detail::prefix<std::tera,	U>::type;			///< Represents the type of `class U` with the metric 'tera' prefix appended.	@details E.g. tera<meters> represents meters*10^12		@tparam U unit type to apply the prefix to.
+	template<class U> using peta	= typename units::detail::prefix<std::peta,	U>::type;			///< Represents the type of `class U` with the metric 'peta' prefix appended.	@details E.g. peta<meters> represents meters*10^15		@tparam U unit type to apply the prefix to.
+	template<class U> using exa		= typename units::detail::prefix<std::exa,	U>::type;			///< Represents the type of `class U` with the metric 'exa' prefix appended.	@details E.g. exa<meters> represents meters*10^18		@tparam U unit type to apply the prefix to.
+	/** @} @} */
+
+	/**
+	 * @ingroup UnitManipulators
+	 * @{
+	 * @ingroup Binary Prefixes
+	 * @{
+	 */
+	template<class U> using kibi	= typename units::detail::prefix<std::ratio<1024>,					U>::type;	///< Represents the type of `class U` with the binary 'kibi' prefix appended.	@details E.g. kibi<bytes> represents bytes*2^10	@tparam U unit type to apply the prefix to.
+	template<class U> using mebi	= typename units::detail::prefix<std::ratio<1048576>,				U>::type;	///< Represents the type of `class U` with the binary 'mibi' prefix appended.	@details E.g. mebi<bytes> represents bytes*2^20	@tparam U unit type to apply the prefix to.
+	template<class U> using gibi	= typename units::detail::prefix<std::ratio<1073741824>,			U>::type;	///< Represents the type of `class U` with the binary 'gibi' prefix appended.	@details E.g. gibi<bytes> represents bytes*2^30	@tparam U unit type to apply the prefix to.
+	template<class U> using tebi	= typename units::detail::prefix<std::ratio<1099511627776>,			U>::type;	///< Represents the type of `class U` with the binary 'tebi' prefix appended.	@details E.g. tebi<bytes> represents bytes*2^40	@tparam U unit type to apply the prefix to.
+	template<class U> using pebi	= typename units::detail::prefix<std::ratio<1125899906842624>,		U>::type;	///< Represents the type of `class U` with the binary 'pebi' prefix appended.	@details E.g. pebi<bytes> represents bytes*2^50	@tparam U unit type to apply the prefix to.
+	template<class U> using exbi	= typename units::detail::prefix<std::ratio<1152921504606846976>,	U>::type;	///< Represents the type of `class U` with the binary 'exbi' prefix appended.	@details E.g. exbi<bytes> represents bytes*2^60	@tparam U unit type to apply the prefix to.
+	/** @} @} */
+
+	//------------------------------
+	//	CONVERSION TRAITS
+	//------------------------------
+
+	namespace traits
+	{
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which checks whether two units can be converted to each other
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_convertible_unit<U1, U2>::value` to test
+		 *				whether `class U1` is convertible to `class U2`. Note: convertible has both the semantic meaning,
+		 *				(i.e. meters can be converted to feet), and the c++ meaning of conversion (type meters can be
+		 *				converted to type feet). Conversion is always symmetric, so if U1 is convertible to U2, then
+		 *				U2 will be convertible to U1.
+		 * @tparam		U1 Unit to convert from.
+		 * @tparam		U2 Unit to convert to.
+		 * @sa			is_convertible_unit_t
+		 */
+		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 >> {};
+	}
+
+	//------------------------------
+	//	CONVERSION FUNCTION
+	//------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		constexpr inline UNIT_LIB_DEFAULT_TYPE pow(UNIT_LIB_DEFAULT_TYPE x, unsigned long long y)
+		{
+			return y == 0 ? 1.0 : x * pow(x, y - 1);
+		}
+
+		constexpr inline UNIT_LIB_DEFAULT_TYPE abs(UNIT_LIB_DEFAULT_TYPE x)
+		{
+			return x < 0 ? -x : x;
+		}
+
+		/// convert dispatch for units which are both the same
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr T convert(const T& value, std::true_type, std::false_type, std::false_type) noexcept
+		{
+			return value;
+		}
+
+		/// convert dispatch for units which are both the same
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr T convert(const T& value, std::true_type, std::false_type, std::true_type) noexcept
+		{
+			return value;
+		}
+
+		/// convert dispatch for units which are both the same
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr T convert(const T& value, std::true_type, std::true_type, std::false_type) noexcept
+		{
+			return value;
+		}
+
+		/// convert dispatch for units which are both the same
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr T convert(const T& value, std::true_type, std::true_type, std::true_type) noexcept
+		{
+			return value;
+		}
+
+		/// convert dispatch for units of different types w/ no translation and no PI
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr T convert(const T& value, std::false_type, std::false_type, std::false_type) noexcept
+		{
+			return ((value * Ratio::num) / Ratio::den);
+		}
+
+		/// convert dispatch for units of different types w/ no translation, but has PI in numerator
+		// constepxr with PI in numerator
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr
+		std::enable_if_t<(PiRatio::num / PiRatio::den >= 1 && PiRatio::num % PiRatio::den == 0), T>
+		convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept
+		{
+			return ((value * pow(constants::detail::PI_VAL, PiRatio::num / PiRatio::den) * Ratio::num) / Ratio::den);
+		}
+
+		/// convert dispatch for units of different types w/ no translation, but has PI in denominator
+		// constexpr with PI in denominator
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr
+		std::enable_if_t<(PiRatio::num / PiRatio::den <= -1 && PiRatio::num % PiRatio::den == 0), T>
+ 		convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept
+ 		{
+ 			return (value * Ratio::num) / (Ratio::den * pow(constants::detail::PI_VAL, -PiRatio::num / PiRatio::den));
+ 		}
+
+		/// convert dispatch for units of different types w/ no translation, but has PI in numerator
+		// Not constexpr - uses std::pow
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline // sorry, this can't be constexpr!
+		std::enable_if_t<(PiRatio::num / PiRatio::den < 1 && PiRatio::num / PiRatio::den > -1), T>
+		convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept
+		{
+			return ((value * std::pow(constants::detail::PI_VAL, PiRatio::num / PiRatio::den)  * Ratio::num) / Ratio::den);
+		}
+
+		/// convert dispatch for units of different types with a translation, but no PI
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr T convert(const T& value, std::false_type, std::false_type, std::true_type) noexcept
+		{
+			return ((value * Ratio::num) / Ratio::den) + (static_cast<UNIT_LIB_DEFAULT_TYPE>(Translation::num) / Translation::den);
+		}
+
+		/// convert dispatch for units of different types with a translation AND PI
+		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
+		static inline constexpr T convert(const T& value, const std::false_type, const std::true_type, const std::true_type) noexcept
+		{
+			return ((value * std::pow(constants::detail::PI_VAL, PiRatio::num / PiRatio::den) * Ratio::num) / Ratio::den) + (static_cast<UNIT_LIB_DEFAULT_TYPE>(Translation::num) / Translation::den);
+		}
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @ingroup		Conversion
+	 * @brief		converts a <i>value</i> from one type to another.
+	 * @details		Converts a <i>value</i> of a built-in arithmetic type to another unit. This does not change
+	 *				the type of <i>value</i>, only what it contains. E.g. @code double result = convert<length::meters, length::feet>(1.0);	// result == 3.28084 @endcode
+	 * @sa			unit_t	for implicit conversion of unit containers.
+	 * @tparam		UnitFrom unit tag to convert <i>value</i> from. Must be a `unit` type (i.e. is_unit<UnitFrom>::value == true),
+	 *				and must be convertible to `UnitTo` (i.e. is_convertible_unit<UnitFrom, UnitTo>::value == true).
+	 * @tparam		UnitTo unit tag to convert <i>value</i> to. Must be a `unit` type (i.e. is_unit<UnitTo>::value == true),
+	 *				and must be convertible from `UnitFrom` (i.e. is_convertible_unit<UnitFrom, UnitTo>::value == true).
+	 * @tparam		T type of <i>value</i>. It is inferred from <i>value</i>, and is expected to be a built-in arithmetic type.
+	 * @param[in]	value Arithmetic value to convert from `UnitFrom` to `UnitTo`. The value should represent
+	 *				a quantity in units of `UnitFrom`.
+	 * @returns		value, converted from units of `UnitFrom` to `UnitTo`.
+	 */
+	template<class UnitFrom, class UnitTo, typename T = UNIT_LIB_DEFAULT_TYPE>
+	static inline constexpr T convert(const T& value) noexcept
+	{
+		static_assert(traits::is_unit<UnitFrom>::value, "Template parameter `UnitFrom` must be a `unit` type.");
+		static_assert(traits::is_unit<UnitTo>::value, "Template parameter `UnitTo` must be a `unit` type.");
+		static_assert(traits::is_convertible_unit<UnitFrom, UnitTo>::value, "Units are not compatible.");
+
+		using Ratio = std::ratio_divide<typename UnitFrom::conversion_ratio, typename UnitTo::conversion_ratio>;
+		using PiRatio = std::ratio_subtract<typename UnitFrom::pi_exponent_ratio, typename UnitTo::pi_exponent_ratio>;
+		using Translation = std::ratio_divide<std::ratio_subtract<typename UnitFrom::translation_ratio, typename UnitTo::translation_ratio>, typename UnitTo::conversion_ratio>;
+
+		using isSame = typename std::is_same<std::decay_t<UnitFrom>, std::decay_t<UnitTo>>::type;
+		using piRequired = std::integral_constant<bool, !(std::is_same<std::ratio<0>, PiRatio>::value)>;
+		using translationRequired = std::integral_constant<bool, !(std::is_same<std::ratio<0>, Translation>::value)>;
+
+		return units::detail::convert<UnitFrom, UnitTo, Ratio, PiRatio, Translation, T>
+			(value, isSame{}, piRequired{}, translationRequired{});
+	}
+
+	//----------------------------------
+	//	NON-LINEAR SCALE TRAITS
+	//----------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace traits
+	{
+		namespace detail
+		{
+			/**
+			* @brief		implementation of has_operator_parenthesis
+			* @details		checks that operator() returns the same type as `Ret`
+			*/
+			template<class T, class Ret>
+			struct has_operator_parenthesis_impl
+			{
+				template<class U>
+				static constexpr auto test(U*) -> decltype(std::declval<U>()()) { return decltype(std::declval<U>()()){}; }
+				template<typename>
+				static constexpr std::false_type test(...) { return std::false_type{}; }
+
+				using type = typename std::is_same<Ret, decltype(test<T>(0))>::type;
+			};
+		}
+
+		/**
+		 * @brief		checks that `class T` has an `operator()` member which returns `Ret`
+		 * @details		used as part of the linear_scale concept.
+		 */
+		template<class T, class Ret>
+		struct has_operator_parenthesis : traits::detail::has_operator_parenthesis_impl<T, Ret>::type {};
+	}
+
+	namespace traits
+	{
+		namespace detail
+		{
+			/**
+			* @brief		implementation of has_value_member
+			* @details		checks for a member named `m_member` with type `Ret`
+			*/
+			template<class T, class Ret>
+			struct has_value_member_impl
+			{
+				template<class U>
+				static constexpr auto test(U* p) -> decltype(p->m_value) { return p->m_value; }
+				template<typename>
+				static constexpr auto test(...)->std::false_type { return std::false_type{}; }
+
+				using type = typename std::is_same<std::decay_t<Ret>, std::decay_t<decltype(test<T>(0))>>::type;
+			};
+		}
+
+		/**
+		 * @brief		checks for a member named `m_member` with type `Ret`
+		 * @details		used as part of the linear_scale concept checker.
+		 */
+		template<class T, class Ret>
+		struct has_value_member : traits::detail::has_value_member_impl<T, Ret>::type {};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace traits
+	{
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests that `class T` meets the requirements for a non-linear scale
+		 * @details		A non-linear scale must:
+		 *				- be default constructible
+		 *				- have an `operator()` member which returns the non-linear value stored in the scale
+		 *				- have an accessible `m_value` member type which stores the linearized value in the scale.
+		 *
+		 *				Linear/nonlinear scales are used by `units::unit` to store values and scale them
+		 *				if they represent things like dB.
+		 */
+		template<class T, class Ret>
+		struct is_nonlinear_scale : std::integral_constant<bool,
+			std::is_default_constructible<T>::value &&
+			has_operator_parenthesis<T, Ret>::value &&
+			has_value_member<T, Ret>::value &&
+			std::is_trivial<T>::value>
+		{};
+	}
+
+	//------------------------------
+	//	UNIT_T TYPE TRAITS
+	//------------------------------
+
+	namespace traits
+	{
+#ifdef FOR_DOXYGEN_PURPOSOES_ONLY
+		/**
+		* @ingroup		TypeTraits
+		* @brief		Trait for accessing the publically defined types of `units::unit_t`
+		* @details		The units library determines certain properties of the unit_t types passed to them
+		*				and what they represent by using the members of the corresponding unit_t_traits instantiation.
+		*/
+		template<typename T>
+		struct unit_t_traits
+		{
+			typedef typename T::non_linear_scale_type non_linear_scale_type;	///< Type of the unit_t non_linear_scale (e.g. linear_scale, decibel_scale). This property is used to enable the proper linear or logarithmic arithmetic functions.
+			typedef typename T::underlying_type underlying_type;				///< Underlying storage type of the `unit_t`, e.g. `double`.
+			typedef typename T::value_type value_type;							///< Synonym for underlying type. May be removed in future versions. Prefer underlying_type.
+			typedef typename T::unit_type unit_type;							///< Type of unit the `unit_t` represents, e.g. `meters`
+		};
+#endif
+
+		/** @cond */	// DOXYGEN IGNORE
+		/**
+		 * @brief		unit_t_traits specialization for things which are not unit_t
+		 * @details
+		 */
+		template<typename T, typename = void>
+		struct unit_t_traits
+		{
+			typedef void non_linear_scale_type;
+			typedef void underlying_type;
+			typedef void value_type;
+			typedef void unit_type;
+		};
+
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait for accessing the publically defined types of `units::unit_t`
+		 * @details
+		 */
+		template<typename T>
+		struct unit_t_traits <T, typename void_t<
+			typename T::non_linear_scale_type,
+			typename T::underlying_type,
+			typename T::value_type,
+			typename T::unit_type>::type>
+		{
+			typedef typename T::non_linear_scale_type non_linear_scale_type;
+			typedef typename T::underlying_type underlying_type;
+			typedef typename T::value_type value_type;
+			typedef typename T::unit_type unit_type;
+		};
+		/** @endcond */	// END DOXYGEN IGNORE
+	}
+
+	namespace traits
+	{
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests whether two container types derived from `unit_t` are convertible to each other
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_convertible_unit_t<U1, U2>::value` to test
+		 *				whether `class U1` is convertible to `class U2`. Note: convertible has both the semantic meaning,
+		 *				(i.e. meters can be converted to feet), and the c++ meaning of conversion (type meters can be
+		 *				converted to type feet). Conversion is always symmetric, so if U1 is convertible to U2, then
+		 *				U2 will be convertible to U1.
+		 * @tparam		U1 Unit to convert from.
+		 * @tparam		U2 Unit to convert to.
+		 * @sa			is_convertible_unit
+		 */
+		template<class U1, class U2>
+		struct is_convertible_unit_t : std::integral_constant<bool,
+			is_convertible_unit<typename units::traits::unit_t_traits<U1>::unit_type, typename units::traits::unit_t_traits<U2>::unit_type>::value>
+		{};
+	}
+
+	//----------------------------------
+	//	UNIT TYPE
+	//----------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	// forward declaration
+	template<typename T> struct linear_scale;
+	template<typename T> struct decibel_scale;
+
+	namespace detail
+	{
+		/**
+		* @brief		helper type to identify units.
+		* @details		A non-templated base class for `unit` which enables RTTI testing.
+		*/
+		struct _unit_t {};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace traits
+	{
+		// forward declaration
+		#if !defined(_MSC_VER) || _MSC_VER > 1800 // bug in VS2013 prevents this from working
+		template<typename... T> struct is_dimensionless_unit;
+		#else
+		template<typename T1, typename T2 = T1, typename T3 = T1> struct is_dimensionless_unit;
+		#endif
+
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Traits which tests if a class is a `unit`
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_unit<T>::value` to test
+		 *				whether `class T` implements a `unit`.
+		 */
+		template<class T>
+		struct is_unit_t : std::is_base_of<units::detail::_unit_t, T>::type {};
+	}
+
+	/**
+	 * @ingroup		UnitContainers
+	 * @brief		Container for values which represent quantities of a given unit.
+	 * @details		Stores a value which represents a quantity in the given units. Unit containers
+	 *				(except scalar values) are *not* convertible to built-in c++ types, in order to
+	 *				provide type safety in dimensional analysis. Unit containers *are* implicitly
+	 *				convertible to other compatible unit container types. Unit containers support
+	 *				various types of arithmetic operations, depending on their scale type.
+	 *
+	 *				The value of a `unit_t` can only be changed on construction, or by assignment
+	 *				from another `unit_t` type. If necessary, the underlying value can be accessed
+	 *				using `operator()`: @code
+	 *				meter_t m(5.0);
+	 *				double val = m(); // val == 5.0	@endcode.
+	 * @tparam		Units unit tag for which type of units the `unit_t` represents (e.g. meters)
+	 * @tparam		T underlying type of the storage. Defaults to double.
+	 * @tparam		NonLinearScale optional scale class for the units. Defaults to linear (i.e. does
+	 *				not scale the unit value). Examples of non-linear scales could be logarithmic,
+	 *				decibel, or richter scales. Non-linear scales must adhere to the non-linear-scale
+	 *				concept, i.e. `is_nonlinear_scale<...>::value` must be `true`.
+	 * @sa
+	 *				- \ref lengthContainers "length unit containers"
+	 *				- \ref massContainers "mass unit containers"
+	 *				- \ref timeContainers "time unit containers"
+	 *				- \ref angleContainers "angle unit containers"
+	 *				- \ref currentContainers "current unit containers"
+	 *				- \ref temperatureContainers "temperature unit containers"
+	 *				- \ref substanceContainers "substance unit containers"
+	 *				- \ref luminousIntensityContainers "luminous intensity unit containers"
+	 *				- \ref solidAngleContainers "solid angle unit containers"
+	 *				- \ref frequencyContainers "frequency unit containers"
+	 *				- \ref velocityContainers "velocity unit containers"
+	 *				- \ref angularVelocityContainers "angular velocity unit containers"
+	 *				- \ref accelerationContainers "acceleration unit containers"
+	 *				- \ref forceContainers "force unit containers"
+	 *				- \ref pressureContainers "pressure unit containers"
+	 *				- \ref chargeContainers "charge unit containers"
+	 *				- \ref energyContainers "energy unit containers"
+	 *				- \ref powerContainers "power unit containers"
+	 *				- \ref voltageContainers "voltage unit containers"
+	 *				- \ref capacitanceContainers "capacitance unit containers"
+	 *				- \ref impedanceContainers "impedance unit containers"
+	 *				- \ref magneticFluxContainers "magnetic flux unit containers"
+	 *				- \ref magneticFieldStrengthContainers "magnetic field strength unit containers"
+	 *				- \ref inductanceContainers "inductance unit containers"
+	 *				- \ref luminousFluxContainers "luminous flux unit containers"
+	 *				- \ref illuminanceContainers "illuminance unit containers"
+	 *				- \ref radiationContainers "radiation unit containers"
+	 *				- \ref torqueContainers "torque unit containers"
+	 *				- \ref areaContainers "area unit containers"
+	 *				- \ref volumeContainers "volume unit containers"
+	 *				- \ref densityContainers "density unit containers"
+	 *				- \ref concentrationContainers "concentration unit containers"
+	 *				- \ref constantContainers "constant unit containers"
+	 */
+	template<class Units, typename T = UNIT_LIB_DEFAULT_TYPE, template<typename> class NonLinearScale = linear_scale>
+	class unit_t : public NonLinearScale<T>, units::detail::_unit_t
+	{
+		static_assert(traits::is_unit<Units>::value, "Template parameter `Units` must be a unit tag. Check that you aren't using a unit type (_t).");
+		static_assert(traits::is_nonlinear_scale<NonLinearScale<T>, T>::value, "Template parameter `NonLinearScale` does not conform to the `is_nonlinear_scale` concept.");
+
+	protected:
+
+		using nls = NonLinearScale<T>;
+		using nls::m_value;
+
+	public:
+
+		typedef NonLinearScale<T> non_linear_scale_type;											///< Type of the non-linear scale of the unit_t (e.g. linear_scale)
+		typedef T underlying_type;																	///< Type of the underlying storage of the unit_t (e.g. double)
+		typedef T value_type;																		///< Synonym for underlying type. May be removed in future versions. Prefer underlying_type.
+		typedef Units unit_type;																	///< Type of `unit` the `unit_t` represents (e.g. meters)
+
+		/**
+		 * @ingroup		Constructors
+		 * @brief		default constructor.
+		 */
+		constexpr unit_t() = default;
+
+		/**
+		 * @brief		constructor
+		 * @details		constructs a new unit_t using the non-linear scale's constructor.
+		 * @param[in]	value	unit value magnitude.
+		 * @param[in]	args	additional constructor arguments are forwarded to the non-linear scale constructor. Which
+		 *						args are required depends on which scale is used. For the default (linear) scale,
+		 *						no additional args are necessary.
+		 */
+		template<class... Args>
+		inline explicit constexpr unit_t(const T value, const Args&... args) noexcept : nls(value, args...)
+		{
+
+		}
+
+		/**
+		 * @brief		constructor
+		 * @details		enable implicit conversions from T types ONLY for linear scalar units
+		 * @param[in]	value value of the unit_t
+		 */
+		template<class Ty, class = typename std::enable_if<traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value>::type>
+		inline constexpr unit_t(const Ty value) noexcept : nls(value)
+		{
+
+		}
+
+		/**
+		 * @brief		chrono constructor
+		 * @details		enable implicit conversions from std::chrono::duration types ONLY for time units
+		 * @param[in]	value value of the unit_t
+		 */
+		template<class Rep, class Period, class = std::enable_if_t<std::is_arithmetic<Rep>::value && traits::is_ratio<Period>::value>>
+		inline constexpr unit_t(const std::chrono::duration<Rep, Period>& value) noexcept :
+		nls(units::convert<unit<std::ratio<1,1000000000>, category::time_unit>, Units>(static_cast<T>(std::chrono::duration_cast<std::chrono::nanoseconds>(value).count())))
+		{
+
+		}
+
+		/**
+		 * @brief		copy constructor (converting)
+		 * @details		performs implicit unit conversions if required.
+		 * @param[in]	rhs unit to copy.
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
+		inline constexpr unit_t(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) noexcept :
+		nls(units::convert<UnitsRhs, Units, T>(rhs.m_value), std::true_type() /*store linear value*/)
+		{
+
+		}
+
+		/**
+		 * @brief		assignment
+		 * @details		performs implicit unit conversions if required
+		 * @param[in]	rhs unit to copy.
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
+		inline unit_t& operator=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) noexcept
+		{
+			nls::m_value = units::convert<UnitsRhs, Units, T>(rhs.m_value);
+			return *this;
+		}
+
+		/**
+		* @brief		assignment
+		* @details		performs implicit conversions from built-in types ONLY for scalar units
+		* @param[in]	rhs value to copy.
+		*/
+		template<class Ty, class = std::enable_if_t<traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value>>
+		inline unit_t& operator=(const Ty& rhs) noexcept
+		{
+			nls::m_value = rhs;
+			return *this;
+		}
+
+		/**
+		 * @brief		less-than
+		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
+		 * @param[in]	rhs right-hand side unit for the comparison
+		 * @returns		true IFF the value of `this` is less than the value of `rhs`
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
+		inline constexpr bool operator<(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
+		{
+			return (nls::m_value < units::convert<UnitsRhs, Units>(rhs.m_value));
+		}
+
+		/**
+		 * @brief		less-than or equal
+		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
+		 * @param[in]	rhs right-hand side unit for the comparison
+		 * @returns		true IFF the value of `this` is less than or equal to the value of `rhs`
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
+		inline constexpr bool operator<=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
+		{
+			return (nls::m_value <= units::convert<UnitsRhs, Units>(rhs.m_value));
+		}
+
+		/**
+		 * @brief		greater-than
+		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
+		 * @param[in]	rhs right-hand side unit for the comparison
+		 * @returns		true IFF the value of `this` is greater than the value of `rhs`
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
+		inline constexpr bool operator>(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
+		{
+			return (nls::m_value > units::convert<UnitsRhs, Units>(rhs.m_value));
+		}
+
+		/**
+		 * @brief		greater-than or equal
+		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
+		 * @param[in]	rhs right-hand side unit for the comparison
+		 * @returns		true IFF the value of `this` is greater than or equal to the value of `rhs`
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
+		inline constexpr bool operator>=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
+		{
+			return (nls::m_value >= units::convert<UnitsRhs, Units>(rhs.m_value));
+		}
+
+		/**
+		 * @brief		equality
+		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
+		 * @param[in]	rhs right-hand side unit for the comparison
+		 * @returns		true IFF the value of `this` exactly equal to the value of rhs.
+		 * @note		This may not be suitable for all applications when the underlying_type of unit_t is a double.
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs, std::enable_if_t<std::is_floating_point<T>::value || std::is_floating_point<Ty>::value, int> = 0>
+		inline constexpr bool operator==(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
+		{
+			return detail::abs(nls::m_value - units::convert<UnitsRhs, Units>(rhs.m_value)) < std::numeric_limits<T>::epsilon() *
+				detail::abs(nls::m_value + units::convert<UnitsRhs, Units>(rhs.m_value)) ||
+				detail::abs(nls::m_value - units::convert<UnitsRhs, Units>(rhs.m_value)) < (std::numeric_limits<T>::min)();
+		}
+
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs, std::enable_if_t<std::is_integral<T>::value && std::is_integral<Ty>::value, int> = 0>
+		inline constexpr bool operator==(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
+		{
+			return nls::m_value == units::convert<UnitsRhs, Units>(rhs.m_value);
+		}
+
+		/**
+		 * @brief		inequality
+		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
+		 * @param[in]	rhs right-hand side unit for the comparison
+		 * @returns		true IFF the value of `this` is not equal to the value of rhs.
+		 * @note		This may not be suitable for all applications when the underlying_type of unit_t is a double.
+		 */
+		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
+		inline constexpr bool operator!=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
+		{
+			return !(*this == rhs);
+		}
+
+		/**
+		 * @brief		unit value
+		 * @returns		value of the unit in it's underlying, non-safe type.
+		 */
+		inline constexpr underlying_type value() const noexcept
+		{
+			return static_cast<underlying_type>(*this);
+		}
+
+		/**
+		 * @brief		unit value
+		 * @returns		value of the unit converted to an arithmetic, non-safe type.
+		 */
+		template<typename Ty, class = std::enable_if_t<std::is_arithmetic<Ty>::value>>
+		inline constexpr Ty to() const noexcept
+		{
+			return static_cast<Ty>(*this);
+		}
+
+		/**
+		 * @brief		linearized unit value
+		 * @returns		linearized value of unit which has a non-linear scale. For `unit_t` types with
+		 *				linear scales, this is equivalent to `value`.
+		 */
+		template<typename Ty, class = std::enable_if_t<std::is_arithmetic<Ty>::value>>
+		inline constexpr Ty toLinearized() const noexcept
+		{
+			return static_cast<Ty>(m_value);
+		}
+
+		/**
+		 * @brief		conversion
+		 * @details		Converts to a different unit container. Units can be converted to other containers
+		 *				implicitly, but this can be used in cases where explicit notation of a conversion
+		 *				is beneficial, or where an r-value container is needed.
+		 * @tparam		U unit (not unit_t) to convert to
+		 * @returns		a unit container with the specified units containing the equivalent value to
+		 *				*this.
+		 */
+		template<class U>
+		inline constexpr unit_t<U> convert() const noexcept
+		{
+			static_assert(traits::is_unit<U>::value, "Template parameter `U` must be a unit type.");
+			return unit_t<U>(*this);
+		}
+
+		/**
+		 * @brief		implicit type conversion.
+		 * @details		only enabled for scalar unit types.
+		 */
+		template<class Ty, std::enable_if_t<traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value, int> = 0>
+		inline constexpr operator Ty() const noexcept
+		{
+			// this conversion also resolves any PI exponents, by converting from a non-zero PI ratio to a zero-pi ratio.
+			return static_cast<Ty>(units::convert<Units, unit<std::ratio<1>, units::category::scalar_unit>>((*this)()));
+		}
+
+		/**
+		 * @brief		explicit type conversion.
+		 * @details		only enabled for non-dimensionless unit types.
+		 */
+		template<class Ty, std::enable_if_t<!traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value, int> = 0>
+		inline constexpr explicit operator Ty() const noexcept
+		{
+			return static_cast<Ty>((*this)());
+		}
+
+		/**
+		 * @brief		chrono implicit type conversion.
+		 * @details		only enabled for time unit types.
+		 */
+		template<typename U = Units, std::enable_if_t<units::traits::is_convertible_unit<U, unit<std::ratio<1>, category::time_unit>>::value, int> = 0>
+		inline constexpr operator std::chrono::nanoseconds() const noexcept
+		{
+			return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double, std::nano>(units::convert<Units, unit<std::ratio<1,1000000000>, category::time_unit>>((*this)())));
+		}
+
+		/**
+		 * @brief		returns the unit name
+		 */
+		inline constexpr const char* name() const noexcept
+		{
+			return units::name(*this);
+		}
+
+		/**
+		 * @brief		returns the unit abbreviation
+		 */
+		inline constexpr const char* abbreviation() const noexcept
+		{
+			return units::abbreviation(*this);
+		}
+
+	public:
+
+		template<class U, typename Ty, template<typename> class Nlt>
+		friend class unit_t;
+	};
+
+	//------------------------------
+	//	UNIT_T NON-MEMBER FUNCTIONS
+	//------------------------------
+
+	/**
+	 * @ingroup		UnitContainers
+	 * @brief		Constructs a unit container from an arithmetic type.
+	 * @details		make_unit can be used to construct a unit container from an arithmetic type, as an alternative to
+	 *				using the explicit constructor. Unlike the explicit constructor it forces the user to explicitly
+	 *				specify the units.
+	 * @tparam		UnitType Type to construct.
+	 * @tparam		Ty		Arithmetic type.
+	 * @param[in]	value	Arithmetic value that represents a quantity in units of `UnitType`.
+	 */
+	template<class UnitType, typename T, class = std::enable_if_t<std::is_arithmetic<T>::value>>
+	inline constexpr UnitType make_unit(const T value) noexcept
+	{
+		static_assert(traits::is_unit_t<UnitType>::value, "Template parameter `UnitType` must be a unit type (_t).");
+
+		return UnitType(value);
+	}
+
+#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
+	template<class Units, typename T, template<typename> class NonLinearScale>
+	inline std::ostream& operator<<(std::ostream& os, const unit_t<Units, T, NonLinearScale>& obj) noexcept
+	{
+		using BaseUnits = unit<std::ratio<1>, typename traits::unit_traits<Units>::base_unit_type>;
+		os << convert<Units, BaseUnits>(obj());
+
+		if (traits::unit_traits<Units>::base_unit_type::meter_ratio::num != 0) { os << " m"; }
+		if (traits::unit_traits<Units>::base_unit_type::meter_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::meter_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::meter_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::meter_ratio::den != 1) { os << "/"   << traits::unit_traits<Units>::base_unit_type::meter_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num != 0) { os << " kg"; }
+		if (traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::kilogram_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::kilogram_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::second_ratio::num != 0) { os << " s"; }
+		if (traits::unit_traits<Units>::base_unit_type::second_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::second_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::second_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::second_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::second_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::ampere_ratio::num != 0) { os << " A"; }
+		if (traits::unit_traits<Units>::base_unit_type::ampere_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::ampere_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::ampere_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::ampere_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::ampere_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num != 0) { os << " K"; }
+		if (traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::kelvin_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::kelvin_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::mole_ratio::num != 0) { os << " mol"; }
+		if (traits::unit_traits<Units>::base_unit_type::mole_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::mole_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::mole_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::mole_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::mole_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::candela_ratio::num != 0) { os << " cd"; }
+		if (traits::unit_traits<Units>::base_unit_type::candela_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::candela_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::candela_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::candela_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::candela_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::radian_ratio::num != 0) { os << " rad"; }
+		if (traits::unit_traits<Units>::base_unit_type::radian_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::radian_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::radian_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::radian_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::radian_ratio::den; }
+
+		if (traits::unit_traits<Units>::base_unit_type::byte_ratio::num != 0) { os << " b"; }
+		if (traits::unit_traits<Units>::base_unit_type::byte_ratio::num != 0 &&
+			traits::unit_traits<Units>::base_unit_type::byte_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::byte_ratio::num; }
+		if (traits::unit_traits<Units>::base_unit_type::byte_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::byte_ratio::den; }
+
+		return os;
+	}
+#endif
+
+	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
+	inline unit_t<Units, T, NonLinearScale>& operator+=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
+	{
+		static_assert(traits::is_convertible_unit_t<unit_t<Units, T, NonLinearScale>, RhsType>::value ||
+			(traits::is_dimensionless_unit<decltype(lhs)>::value && std::is_arithmetic<RhsType>::value),
+			"parameters are not compatible units.");
+
+		lhs = lhs + rhs;
+		return lhs;
+	}
+
+	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
+	inline unit_t<Units, T, NonLinearScale>& operator-=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
+	{
+		static_assert(traits::is_convertible_unit_t<unit_t<Units, T, NonLinearScale>, RhsType>::value ||
+			(traits::is_dimensionless_unit<decltype(lhs)>::value && std::is_arithmetic<RhsType>::value),
+			"parameters are not compatible units.");
+
+		lhs = lhs - rhs;
+		return lhs;
+	}
+
+	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
+	inline unit_t<Units, T, NonLinearScale>& operator*=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
+	{
+		static_assert((traits::is_dimensionless_unit<RhsType>::value || std::is_arithmetic<RhsType>::value),
+			"right-hand side parameter must be dimensionless.");
+
+		lhs = lhs * rhs;
+		return lhs;
+	}
+
+	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
+	inline unit_t<Units, T, NonLinearScale>& operator/=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
+	{
+		static_assert((traits::is_dimensionless_unit<RhsType>::value || std::is_arithmetic<RhsType>::value),
+			"right-hand side parameter must be dimensionless.");
+
+		lhs = lhs / rhs;
+		return lhs;
+	}
+
+	//------------------------------
+	//	UNIT_T UNARY OPERATORS
+	//------------------------------
+
+	// unary addition: +T
+	template<class Units, typename T, template<typename> class NonLinearScale>
+	inline unit_t<Units, T, NonLinearScale> operator+(const unit_t<Units, T, NonLinearScale>& u) noexcept
+	{
+		return u;
+	}
+
+	// prefix increment: ++T
+	template<class Units, typename T, template<typename> class NonLinearScale>
+	inline unit_t<Units, T, NonLinearScale>& operator++(unit_t<Units, T, NonLinearScale>& u) noexcept
+	{
+		u = unit_t<Units, T, NonLinearScale>(u() + 1);
+		return u;
+	}
+
+	// postfix increment: T++
+	template<class Units, typename T, template<typename> class NonLinearScale>
+	inline unit_t<Units, T, NonLinearScale> operator++(unit_t<Units, T, NonLinearScale>& u, int) noexcept
+	{
+		auto ret = u;
+		u = unit_t<Units, T, NonLinearScale>(u() + 1);
+		return ret;
+	}
+
+	// unary addition: -T
+	template<class Units, typename T, template<typename> class NonLinearScale>
+	inline unit_t<Units, T, NonLinearScale> operator-(const unit_t<Units, T, NonLinearScale>& u) noexcept
+	{
+		return unit_t<Units, T, NonLinearScale>(-u());
+	}
+
+	// prefix increment: --T
+	template<class Units, typename T, template<typename> class NonLinearScale>
+	inline unit_t<Units, T, NonLinearScale>& operator--(unit_t<Units, T, NonLinearScale>& u) noexcept
+	{
+		u = unit_t<Units, T, NonLinearScale>(u() - 1);
+		return u;
+	}
+
+	// postfix increment: T--
+	template<class Units, typename T, template<typename> class NonLinearScale>
+	inline unit_t<Units, T, NonLinearScale> operator--(unit_t<Units, T, NonLinearScale>& u, int) noexcept
+	{
+		auto ret = u;
+		u = unit_t<Units, T, NonLinearScale>(u() - 1);
+		return ret;
+	}
+
+	//------------------------------
+	//	UNIT_CAST
+	//------------------------------
+
+	/**
+	 * @ingroup		Conversion
+	 * @brief		Casts a unit container to an arithmetic type.
+	 * @details		unit_cast can be used to remove the strong typing from a unit class, and convert it
+	 *				to a built-in arithmetic type. This may be useful for compatibility with libraries
+	 *				and legacy code that don't support `unit_t` types. E.g
+	 * @code		meter_t unitVal(5);
+	 *  double value = units::unit_cast<double>(unitVal);	// value = 5.0
+	 * @endcode
+	 * @tparam		T		Type to cast the unit type to. Must be a built-in arithmetic type.
+	 * @param		value	Unit value to cast.
+	 * @sa			unit_t::to
+	 */
+	template<typename T, typename Units, class = std::enable_if_t<std::is_arithmetic<T>::value && traits::is_unit_t<Units>::value>>
+	inline constexpr T unit_cast(const Units& value) noexcept
+	{
+		return static_cast<T>(value);
+	}
+
+	//------------------------------
+	//	NON-LINEAR SCALE TRAITS
+	//------------------------------
+
+	// forward declaration
+	template<typename T> struct decibel_scale;
+
+	namespace traits
+	{
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests whether a type is inherited from a linear scale.
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_linear_scale<U1 [, U2, ...]>::value` to test
+		 *				one or more types to see if they represent unit_t's whose scale is linear.
+		 * @tparam		T	one or more types to test.
+		 */
+#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 > {};
+#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> {};
+#endif
+
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests whether a type is inherited from a decibel scale.
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_decibel_scale<U1 [, U2, ...]>::value` to test
+		 *				one or more types to see if they represent unit_t's whose scale is in decibels.
+		 * @tparam		T	one or more types to test.
+		 */
+#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> {};
+#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> {};
+#endif
+
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests whether two types has the same non-linear scale.
+		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_same_scale<U1 , U2>::value` to test
+		 *				whether two types have the same non-linear scale.
+		 * @tparam		T1	left hand type.
+		 * @tparam		T2	right hand type
+		 */
+		template<typename T1, typename T2>
+		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>
+		{};
+	}
+
+	//----------------------------------
+	//	NON-LINEAR SCALES
+	//----------------------------------
+
+	// Non-linear transforms are used to pre and post scale units which are defined in terms of non-
+	// linear functions of their current value. A good example of a non-linear scale would be a
+	// logarithmic or decibel scale
+
+	//------------------------------
+	//	LINEAR SCALE
+	//------------------------------
+
+	/**
+	 * @brief		unit_t scale which is linear
+	 * @details		Represents units on a linear scale. This is the appropriate unit_t scale for almost
+	 *				all units almost all of the time.
+	 * @tparam		T	underlying storage type
+	 * @sa			unit_t
+	 */
+	template<typename T>
+	struct linear_scale
+	{
+		inline constexpr linear_scale() = default;													///< default constructor.
+		inline constexpr linear_scale(const linear_scale&) = default;
+		inline ~linear_scale() = default;
+		inline linear_scale& operator=(const linear_scale&) = default;
+#if defined(_MSC_VER) && (_MSC_VER > 1800)
+		inline constexpr linear_scale(linear_scale&&) = default;
+		inline linear_scale& operator=(linear_scale&&) = default;
+#endif
+		template<class... Args>
+		inline constexpr linear_scale(const T& value, Args&&...) noexcept : m_value(value) {}	///< constructor.
+		inline constexpr T operator()() const noexcept { return m_value; }							///< returns value.
+
+		T m_value;																					///< linearized value.
+	};
+
+	//----------------------------------
+	//	SCALAR (LINEAR) UNITS
+	//----------------------------------
+
+	// Scalar units are the *ONLY* units implicitly convertible to/from built-in types.
+	namespace dimensionless
+	{
+		typedef unit<std::ratio<1>, units::category::scalar_unit> scalar;
+		typedef unit<std::ratio<1>, units::category::dimensionless_unit> dimensionless;
+
+		typedef unit_t<scalar> scalar_t;
+		typedef scalar_t dimensionless_t;
+	}
+
+// ignore the redeclaration of the default template parameters
+#if defined(_MSC_VER)
+#	pragma warning(push)
+#	pragma warning(disable : 4348)
+#endif
+	UNIT_ADD_CATEGORY_TRAIT(scalar)
+	UNIT_ADD_CATEGORY_TRAIT(dimensionless)
+#if defined(_MSC_VER)
+#	pragma warning(pop)
+#endif
+
+	//------------------------------
+	//	LINEAR ARITHMETIC
+	//------------------------------
+
+	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<!traits::is_same_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
+	constexpr inline int operator+(const UnitTypeLhs& /* lhs */, const UnitTypeRhs& /* rhs */) noexcept
+	{
+		static_assert(traits::is_same_scale<UnitTypeLhs, UnitTypeRhs>::value, "Cannot add units with different linear/non-linear scales.");
+		return 0;
+	}
+
+	/// Addition operator for unit_t types with a linear_scale.
+	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
+	inline constexpr UnitTypeLhs operator+(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		return UnitTypeLhs(lhs() + convert<UnitsRhs, UnitsLhs>(rhs()));
+	}
+
+	/// Addition operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
+	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
+	inline constexpr dimensionless::scalar_t operator+(const dimensionless::scalar_t& lhs, T rhs) noexcept
+	{
+		return dimensionless::scalar_t(lhs() + rhs);
+	}
+
+	/// Addition operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
+	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
+	inline constexpr dimensionless::scalar_t operator+(T lhs, const dimensionless::scalar_t& rhs) noexcept
+	{
+		return dimensionless::scalar_t(lhs + rhs());
+	}
+
+	/// Subtraction operator for unit_t types with a linear_scale.
+	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
+	inline constexpr UnitTypeLhs operator-(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		return UnitTypeLhs(lhs() - convert<UnitsRhs, UnitsLhs>(rhs()));
+	}
+
+	/// Subtraction operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
+	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
+	inline constexpr dimensionless::scalar_t operator-(const dimensionless::scalar_t& lhs, T rhs) noexcept
+	{
+		return dimensionless::scalar_t(lhs() - rhs);
+	}
+
+	/// Subtraction operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
+	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
+	inline constexpr dimensionless::scalar_t operator-(T lhs, const dimensionless::scalar_t& rhs) noexcept
+	{
+		return dimensionless::scalar_t(lhs - rhs());
+	}
+
+	/// Multiplication type for convertible unit_t types with a linear scale. @returns the multiplied value, with the same type as left-hand side unit.
+	template<class UnitTypeLhs, class UnitTypeRhs,
+		std::enable_if_t<traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
+		inline constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<squared<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>>>
+	{
+		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		return  unit_t<compound_unit<squared<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>>>
+			(lhs() * convert<UnitsRhs, UnitsLhs>(rhs()));
+	}
+
+	/// Multiplication type for non-convertible unit_t types with a linear scale. @returns the multiplied value, whose type is a compound unit of the left and right hand side values.
+	template<class UnitTypeLhs, class UnitTypeRhs,
+		std::enable_if_t<!traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+		inline constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type, typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
+	{
+		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		return unit_t<compound_unit<UnitsLhs, UnitsRhs>>
+			(lhs() * rhs());
+	}
+
+	/// Multiplication by a dimensionless unit for unit_t types with a linear scale.
+	template<class UnitTypeLhs, typename UnitTypeRhs,
+		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+		inline constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		// the cast makes sure factors of PI are handled as expected
+		return UnitTypeLhs(lhs() * static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
+	}
+
+	/// Multiplication by a dimensionless unit for unit_t types with a linear scale.
+	template<class UnitTypeLhs, typename UnitTypeRhs,
+		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+		inline constexpr UnitTypeRhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		// the cast makes sure factors of PI are handled as expected
+		return UnitTypeRhs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) * rhs());
+	}
+
+	/// Multiplication by a scalar for unit_t types with a linear scale.
+	template<class UnitTypeLhs, typename T,
+		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeLhs>::value, int> = 0>
+		inline constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, T rhs) noexcept
+	{
+		return UnitTypeLhs(lhs() * rhs);
+	}
+
+	/// Multiplication by a scalar for unit_t types with a linear scale.
+	template<class UnitTypeRhs, typename T,
+		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeRhs>::value, int> = 0>
+		inline constexpr UnitTypeRhs operator*(T lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		return UnitTypeRhs(lhs * rhs());
+	}
+
+	/// Division for convertible unit_t types with a linear scale. @returns the lhs divided by rhs value, whose type is a scalar
+	template<class UnitTypeLhs, class UnitTypeRhs,
+		std::enable_if_t<traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
+		inline constexpr dimensionless::scalar_t operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		return dimensionless::scalar_t(lhs() / convert<UnitsRhs, UnitsLhs>(rhs()));
+	}
+
+	/// Division for non-convertible unit_t types with a linear scale. @returns the lhs divided by the rhs, with a compound unit type of lhs/rhs
+	template<class UnitTypeLhs, class UnitTypeRhs,
+		std::enable_if_t<!traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+		inline constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept ->  unit_t<compound_unit<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type, inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>>
+	{
+		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		return unit_t<compound_unit<UnitsLhs, inverse<UnitsRhs>>>
+			(lhs() / rhs());
+	}
+
+	/// Division by a dimensionless unit for unit_t types with a linear scale
+	template<class UnitTypeLhs, class UnitTypeRhs,
+		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+		inline constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		return UnitTypeLhs(lhs() / static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
+	}
+
+	/// Division of a dimensionless unit  by a unit_t type with a linear scale
+	template<class UnitTypeLhs, class UnitTypeRhs,
+		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+		inline constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
+	{
+		return unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
+			(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) / rhs());
+	}
+
+	/// Division by a scalar for unit_t types with a linear scale
+	template<class UnitTypeLhs, typename T,
+		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeLhs>::value, int> = 0>
+		inline constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, T rhs) noexcept
+	{
+		return UnitTypeLhs(lhs() / rhs);
+	}
+
+	/// Division of a scalar  by a unit_t type with a linear scale
+	template<class UnitTypeRhs, typename T,
+		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeRhs>::value, int> = 0>
+		inline constexpr auto operator/(T lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
+	{
+		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		return unit_t<inverse<UnitsRhs>>
+			(lhs / rhs());
+	}
+
+	//----------------------------------
+	//	SCALAR COMPARISONS
+	//----------------------------------
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator==(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
+	{
+		return detail::abs(lhs - static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::epsilon() * detail::abs(lhs + static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) ||
+			detail::abs(lhs - static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) < (std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min)();
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator==(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
+	{
+		return detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) - rhs) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::epsilon() * detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) + rhs) ||
+			detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) - rhs) < (std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min)();
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator!=(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
+	{
+		return!(lhs == static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator!=(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
+	{
+		return !(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) == rhs);
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator>=(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
+	{
+		return std::isgreaterequal(lhs, static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator>=(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
+	{
+		return std::isgreaterequal(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs), rhs);
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator>(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
+	{
+		return lhs > static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs);
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator>(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
+	{
+		return static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) > rhs;
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator<=(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
+	{
+		return std::islessequal(lhs, static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator<=(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
+	{
+		return std::islessequal(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs), rhs);
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator<(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
+	{
+		return lhs < static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs);
+	}
+
+	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
+	constexpr bool operator<(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
+	{
+		return static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) < rhs;
+	}
+
+	//----------------------------------
+	//	POW
+	//----------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		/// recursive exponential implementation
+		template <int N, class U> struct power_of_unit
+		{
+			typedef typename units::detail::unit_multiply<U, typename power_of_unit<N - 1, U>::type> type;
+		};
+
+		/// End recursion
+		template <class U> struct power_of_unit<1, U>
+		{
+			typedef U type;
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace math
+	{
+		/**
+		 * @brief		computes the value of <i>value</i> raised to the <i>power</i>
+		 * @details		Only implemented for linear_scale units. <i>Power</i> must be known at compile time, so the resulting unit type can be deduced.
+		 * @tparam		power exponential power to raise <i>value</i> by.
+		 * @param[in]	value `unit_t` derived type to raise to the given <i>power</i>
+		 * @returns		new unit_t, raised to the given exponent
+		 */
+		template<int power, class UnitType, class = typename std::enable_if<traits::has_linear_scale<UnitType>::value, int>>
+		inline auto pow(const UnitType& value) noexcept -> unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
+		{
+			return unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
+				(std::pow(value(), power));
+		}
+
+		/**
+		 * @brief		computes the value of <i>value</i> raised to the <i>power</i> as a constexpr
+		 * @details		Only implemented for linear_scale units. <i>Power</i> must be known at compile time, so the resulting unit type can be deduced.
+		 *				Additionally, the power must be <i>a positive, integral, value</i>.
+		 * @tparam		power exponential power to raise <i>value</i> by.
+		 * @param[in]	value `unit_t` derived type to raise to the given <i>power</i>
+		 * @returns		new unit_t, raised to the given exponent
+		 */
+		template<int power, class UnitType, class = typename std::enable_if<traits::has_linear_scale<UnitType>::value, int>>
+		inline constexpr auto cpow(const UnitType& value) noexcept -> unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
+		{
+			static_assert(power >= 0, "cpow cannot accept negative numbers. Try units::math::pow instead.");
+			return unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
+				(detail::pow(value(), power));
+		}
+	}
+
+	//------------------------------
+	//	DECIBEL SCALE
+	//------------------------------
+
+	/**
+	* @brief		unit_t scale for representing decibel values.
+	* @details		internally stores linearized values. `operator()` returns the value in dB.
+	* @tparam		T	underlying storage type
+	* @sa			unit_t
+	*/
+	template<typename T>
+	struct decibel_scale
+	{
+		inline constexpr decibel_scale() = default;
+		inline constexpr decibel_scale(const decibel_scale&) = default;
+		inline ~decibel_scale() = default;
+		inline decibel_scale& operator=(const decibel_scale&) = default;
+#if defined(_MSC_VER) && (_MSC_VER > 1800)
+		inline constexpr decibel_scale(decibel_scale&&) = default;
+		inline decibel_scale& operator=(decibel_scale&&) = default;
+#endif
+		inline constexpr decibel_scale(const T value) noexcept : m_value(std::pow(10, value / 10)) {}
+		template<class... Args>
+		inline constexpr decibel_scale(const T value, std::true_type, Args&&...) noexcept : m_value(value) {}
+		inline constexpr T operator()() const noexcept { return 10 * std::log10(m_value); }
+
+		T m_value;	///< linearized value
+	};
+
+	//------------------------------
+	//	SCALAR (DECIBEL) UNITS
+	//------------------------------
+
+	/**
+	 * @brief		namespace for unit types and containers for units that have no dimension (scalar units)
+	 * @sa			See unit_t for more information on unit type containers.
+	 */
+	namespace dimensionless
+	{
+		typedef unit_t<scalar, UNIT_LIB_DEFAULT_TYPE, decibel_scale> dB_t;
+#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
+		inline std::ostream& operator<<(std::ostream& os, const dB_t& obj) { os << obj() << " dB"; return os; }
+#endif
+		typedef dB_t dBi_t;
+	}
+
+	//------------------------------
+	//	DECIBEL ARITHMETIC
+	//------------------------------
+
+	/// Addition for convertible unit_t types with a decibel_scale
+	template<class UnitTypeLhs, class UnitTypeRhs,
+		std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
+	constexpr inline auto operator+(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<squared<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>>, typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type, decibel_scale>
+	{
+		using LhsUnits = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using RhsUnits = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
+
+		return unit_t<compound_unit<squared<LhsUnits>>, underlying_type, decibel_scale>
+			(lhs.template toLinearized<underlying_type>() * convert<RhsUnits, LhsUnits>(rhs.template toLinearized<underlying_type>()), std::true_type());
+	}
+
+	/// Addition between unit_t types with a decibel_scale and dimensionless dB units
+	template<class UnitTypeLhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value, int> = 0>
+	constexpr inline UnitTypeLhs operator+(const UnitTypeLhs& lhs, const dimensionless::dB_t& rhs) noexcept
+	{
+		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
+		return UnitTypeLhs(lhs.template toLinearized<underlying_type>() * rhs.template toLinearized<underlying_type>(), std::true_type());
+	}
+
+	/// Addition between unit_t types with a decibel_scale and dimensionless dB units
+	template<class UnitTypeRhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+	constexpr inline UnitTypeRhs operator+(const dimensionless::dB_t& lhs, const UnitTypeRhs& rhs) noexcept
+	{
+		using underlying_type = typename units::traits::unit_t_traits<UnitTypeRhs>::underlying_type;
+		return UnitTypeRhs(lhs.template toLinearized<underlying_type>() * rhs.template toLinearized<underlying_type>(), std::true_type());
+	}
+
+	/// Subtraction for convertible unit_t types with a decibel_scale
+	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
+	constexpr inline auto operator-(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type, inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>, typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type, decibel_scale>
+	{
+		using LhsUnits = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
+		using RhsUnits = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
+
+		return unit_t<compound_unit<LhsUnits, inverse<RhsUnits>>, underlying_type, decibel_scale>
+			(lhs.template toLinearized<underlying_type>() / convert<RhsUnits, LhsUnits>(rhs.template toLinearized<underlying_type>()), std::true_type());
+	}
+
+	/// Subtraction between unit_t types with a decibel_scale and dimensionless dB units
+	template<class UnitTypeLhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value, int> = 0>
+	constexpr inline UnitTypeLhs operator-(const UnitTypeLhs& lhs, const dimensionless::dB_t& rhs) noexcept
+	{
+		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
+		return UnitTypeLhs(lhs.template toLinearized<underlying_type>() / rhs.template toLinearized<underlying_type>(), std::true_type());
+	}
+
+	/// Subtraction between unit_t types with a decibel_scale and dimensionless dB units
+	template<class UnitTypeRhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
+	constexpr inline auto operator-(const dimensionless::dB_t& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>, typename units::traits::unit_t_traits<UnitTypeRhs>::underlying_type, decibel_scale>
+	{
+		using RhsUnits = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
+		using underlying_type = typename units::traits::unit_t_traits<RhsUnits>::underlying_type;
+
+		return unit_t<inverse<RhsUnits>, underlying_type, decibel_scale>
+			(lhs.template toLinearized<underlying_type>() / rhs.template toLinearized<underlying_type>(), std::true_type());
+	}
+
+	//----------------------------------
+	//	UNIT RATIO CLASS
+	//----------------------------------
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		template<class Units>
+		struct _unit_value_t {};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	namespace traits
+	{
+#ifdef FOR_DOXYGEN_PURPOSES_ONLY
+		/**
+		* @ingroup		TypeTraits
+		* @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
+		* @details		The units library determines certain properties of the `unit_value_t` types passed to
+		*				them and what they represent by using the members of the corresponding `unit_value_t_traits`
+		*				instantiation.
+		*/
+		template<typename T>
+		struct unit_value_t_traits
+		{
+			typedef typename T::unit_type unit_type;	///< Dimension represented by the `unit_value_t`.
+			typedef typename T::ratio ratio;			///< Quantity represented by the `unit_value_t`, expressed as arational number.
+		};
+#endif
+
+		/** @cond */	// DOXYGEN IGNORE
+		/**
+		 * @brief		unit_value_t_traits specialization for things which are not unit_t
+		 * @details
+		 */
+		template<typename T, typename = void>
+		struct unit_value_t_traits
+		{
+			typedef void unit_type;
+			typedef void ratio;
+		};
+
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
+		 * @details
+		 */
+		template<typename T>
+		struct unit_value_t_traits <T, typename void_t<
+			typename T::unit_type,
+			typename T::ratio>::type>
+		{
+			typedef typename T::unit_type unit_type;
+			typedef typename T::ratio ratio;
+		};
+		/** @endcond */	// END DOXYGEN IGNORE
+	}
+
+	//------------------------------------------------------------------------------
+	//	COMPILE-TIME UNIT VALUES AND ARITHMETIC
+	//------------------------------------------------------------------------------
+
+	/**
+	 * @ingroup		UnitContainers
+	 * @brief		Stores a rational unit value as a compile-time constant
+	 * @details		unit_value_t is useful for performing compile-time arithmetic on known
+	 *				unit quantities.
+	 * @tparam		Units	units represented by the `unit_value_t`
+	 * @tparam		Num		numerator of the represented value.
+	 * @tparam		Denom	denominator of the represented value.
+	 * @sa			unit_value_t_traits to access information about the properties of the class,
+	 *				such as it's unit type and rational value.
+	 * @note		This is intentionally identical in concept to a `std::ratio`.
+	 *
+	 */
+	template<typename Units, std::uintmax_t Num, std::uintmax_t Denom = 1>
+	struct unit_value_t : units::detail::_unit_value_t<Units>
+	{
+		typedef Units unit_type;
+		typedef std::ratio<Num, Denom> ratio;
+
+		static_assert(traits::is_unit<Units>::value, "Template parameter `Units` must be a unit type.");
+		static constexpr const unit_t<Units> value() { return unit_t<Units>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den); }
+	};
+
+	namespace traits
+	{
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests whether a type is a unit_value_t representing the given unit type.
+		 * @details		e.g. `is_unit_value_t<meters, myType>::value` would test that `myType` is a
+		 *				`unit_value_t<meters>`.
+		 * @tparam		Units	units that the `unit_value_t` is supposed to have.
+		 * @tparam		T		type to test.
+		 */
+		template<typename T, typename Units = typename traits::unit_value_t_traits<T>::unit_type>
+		struct is_unit_value_t : std::integral_constant<bool,
+			std::is_base_of<units::detail::_unit_value_t<Units>, T>::value>
+		{};
+
+		/**
+		 * @ingroup		TypeTraits
+		 * @brief		Trait which tests whether type T is a unit_value_t with a unit type in the given category.
+		 * @details		e.g. `is_unit_value_t_category<units::category::length, unit_value_t<feet>>::value` would be true
+		 */
+		template<typename Category, typename T>
+		struct is_unit_value_t_category : std::integral_constant<bool,
+			std::is_same<units::traits::base_unit_of<typename traits::unit_value_t_traits<T>::unit_type>, Category>::value>
+		{
+			static_assert(is_base_unit<Category>::value, "Template parameter `Category` must be a `base_unit` type.");
+		};
+	}
+
+	/** @cond */	// DOXYGEN IGNORE
+	namespace detail
+	{
+		// base class for common arithmetic
+		template<class U1, class U2>
+		struct unit_value_arithmetic
+		{
+			static_assert(traits::is_unit_value_t<U1>::value, "Template parameter `U1` must be a `unit_value_t` type.");
+			static_assert(traits::is_unit_value_t<U2>::value, "Template parameter `U2` must be a `unit_value_t` type.");
+
+			using _UNIT1 = typename traits::unit_value_t_traits<U1>::unit_type;
+			using _UNIT2 = typename traits::unit_value_t_traits<U2>::unit_type;
+			using _CONV1 = typename units::traits::unit_traits<_UNIT1>::conversion_ratio;
+			using _CONV2 = typename units::traits::unit_traits<_UNIT2>::conversion_ratio;
+			using _RATIO1 = typename traits::unit_value_t_traits<U1>::ratio;
+			using _RATIO2 = typename traits::unit_value_t_traits<U2>::ratio;
+			using _RATIO2CONV = typename std::ratio_divide<std::ratio_multiply<_RATIO2, _CONV2>, _CONV1>;
+			using _PI_EXP = std::ratio_subtract<typename units::traits::unit_traits<_UNIT2>::pi_exponent_ratio, typename units::traits::unit_traits<_UNIT1>::pi_exponent_ratio>;
+		};
+	}
+	/** @endcond */	// END DOXYGEN IGNORE
+
+	/**
+	 * @ingroup		CompileTimeUnitManipulators
+	 * @brief		adds two unit_value_t types at compile-time
+	 * @details		The resulting unit will the the `unit_type` of `U1`
+	 * @tparam		U1	left-hand `unit_value_t`
+	 * @tparam		U2	right-hand `unit_value_t`
+	 * @sa			unit_value_t_traits to access information about the properties of the class,
+	 *				such as it's unit type and rational value.
+	 * @note		very similar in concept to `std::ratio_add`
+	 */
+	template<class U1, class U2>
+	struct unit_value_add : units::detail::unit_value_arithmetic<U1, U2>, units::detail::_unit_value_t<typename traits::unit_value_t_traits<U1>::unit_type>
+	{
+		/** @cond */	// DOXYGEN IGNORE
+		using Base = units::detail::unit_value_arithmetic<U1, U2>;
+		typedef typename Base::_UNIT1 unit_type;
+		using ratio = std::ratio_add<typename Base::_RATIO1, typename Base::_RATIO2CONV>;
+
+		static_assert(traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, "Unit types are not compatible.");
+		/** @endcond */	// END DOXYGEN IGNORE
+
+		/**
+		 * @brief		Value of sum
+		 * @details		Returns the calculated value of the sum of `U1` and `U2`, in the same
+		 *				units as `U1`.
+		 * @returns		Value of the sum in the appropriate units.
+		 */
+		static constexpr const unit_t<unit_type> value() noexcept
+		{
+			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
+			return value(UsePi());
+		}
+
+		/** @cond */	// DOXYGEN IGNORE
+		// value if PI isn't involved
+		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
+		{
+			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
+		}
+
+		// value if PI *is* involved
+		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
+		{
+			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO1::num / Base::_RATIO1::den) +
+			((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO2CONV::num / Base::_RATIO2CONV::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
+		}
+		/** @endcond */	// END DOXYGEN IGNORE
+	};
+
+	/**
+	 * @ingroup		CompileTimeUnitManipulators
+	 * @brief		subtracts two unit_value_t types at compile-time
+	 * @details		The resulting unit will the the `unit_type` of `U1`
+	 * @tparam		U1	left-hand `unit_value_t`
+	 * @tparam		U2	right-hand `unit_value_t`
+	 * @sa			unit_value_t_traits to access information about the properties of the class,
+	 *				such as it's unit type and rational value.
+	 * @note		very similar in concept to `std::ratio_subtract`
+	 */
+	template<class U1, class U2>
+	struct unit_value_subtract : units::detail::unit_value_arithmetic<U1, U2>, units::detail::_unit_value_t<typename traits::unit_value_t_traits<U1>::unit_type>
+	{
+		/** @cond */	// DOXYGEN IGNORE
+		using Base = units::detail::unit_value_arithmetic<U1, U2>;
+
+		typedef typename Base::_UNIT1 unit_type;
+		using ratio = std::ratio_subtract<typename Base::_RATIO1, typename Base::_RATIO2CONV>;
+
+		static_assert(traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, "Unit types are not compatible.");
+		/** @endcond */	// END DOXYGEN IGNORE
+
+		/**
+		 * @brief		Value of difference
+		 * @details		Returns the calculated value of the difference of `U1` and `U2`, in the same
+		 *				units as `U1`.
+		 * @returns		Value of the difference in the appropriate units.
+		 */
+		static constexpr const unit_t<unit_type> value() noexcept
+		{
+			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
+			return value(UsePi());
+		}
+
+		/** @cond */	// DOXYGEN IGNORE
+		// value if PI isn't involved
+		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
+		{
+			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
+		}
+
+		// value if PI *is* involved
+		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
+		{
+			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO1::num / Base::_RATIO1::den) - ((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO2CONV::num / Base::_RATIO2CONV::den)
+				* std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
+		}
+		/** @endcond */	// END DOXYGEN IGNORE	};
+	};
+
+	/**
+	 * @ingroup		CompileTimeUnitManipulators
+	 * @brief		multiplies two unit_value_t types at compile-time
+	 * @details		The resulting unit will the the `unit_type` of `U1 * U2`
+	 * @tparam		U1	left-hand `unit_value_t`
+	 * @tparam		U2	right-hand `unit_value_t`
+	 * @sa			unit_value_t_traits to access information about the properties of the class,
+	 *				such as it's unit type and rational value.
+	 * @note		very similar in concept to `std::ratio_multiply`
+	 */
+	template<class U1, class U2>
+	struct unit_value_multiply : units::detail::unit_value_arithmetic<U1, U2>,
+		units::detail::_unit_value_t<typename std::conditional<traits::is_convertible_unit<typename traits::unit_value_t_traits<U1>::unit_type,
+			typename traits::unit_value_t_traits<U2>::unit_type>::value, compound_unit<squared<typename traits::unit_value_t_traits<U1>::unit_type>>,
+			compound_unit<typename traits::unit_value_t_traits<U1>::unit_type, typename traits::unit_value_t_traits<U2>::unit_type>>::type>
+	{
+		/** @cond */	// DOXYGEN IGNORE
+		using Base = units::detail::unit_value_arithmetic<U1, U2>;
+
+		using unit_type = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, compound_unit<squared<typename Base::_UNIT1>>, compound_unit<typename Base::_UNIT1, typename Base::_UNIT2>>;
+		using ratio = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, std::ratio_multiply<typename Base::_RATIO1, typename Base::_RATIO2CONV>, std::ratio_multiply<typename Base::_RATIO1, typename Base::_RATIO2>>;
+		/** @endcond */	// END DOXYGEN IGNORE
+
+		/**
+		 * @brief		Value of product
+		 * @details		Returns the calculated value of the product of `U1` and `U2`, in units
+		 *				of `U1 x U2`.
+		 * @returns		Value of the product in the appropriate units.
+		 */
+		static constexpr const unit_t<unit_type> value() noexcept
+		{
+			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
+			return value(UsePi());
+		}
+
+		/** @cond */	// DOXYGEN IGNORE
+		// value if PI isn't involved
+		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
+		{
+			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
+		}
+
+		// value if PI *is* involved
+		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
+		{
+			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
+		}
+		/** @endcond */	// END DOXYGEN IGNORE
+	};
+
+	/**
+	 * @ingroup		CompileTimeUnitManipulators
+	 * @brief		divides two unit_value_t types at compile-time
+	 * @details		The resulting unit will the the `unit_type` of `U1`
+	 * @tparam		U1	left-hand `unit_value_t`
+	 * @tparam		U2	right-hand `unit_value_t`
+	 * @sa			unit_value_t_traits to access information about the properties of the class,
+	 *				such as it's unit type and rational value.
+	 * @note		very similar in concept to `std::ratio_divide`
+	 */
+	template<class U1, class U2>
+	struct unit_value_divide : units::detail::unit_value_arithmetic<U1, U2>,
+		units::detail::_unit_value_t<typename std::conditional<traits::is_convertible_unit<typename traits::unit_value_t_traits<U1>::unit_type,
+		typename traits::unit_value_t_traits<U2>::unit_type>::value, dimensionless::scalar, compound_unit<typename traits::unit_value_t_traits<U1>::unit_type,
+		inverse<typename traits::unit_value_t_traits<U2>::unit_type>>>::type>
+	{
+		/** @cond */	// DOXYGEN IGNORE
+		using Base = units::detail::unit_value_arithmetic<U1, U2>;
+
+		using unit_type = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, dimensionless::scalar, compound_unit<typename Base::_UNIT1, inverse<typename Base::_UNIT2>>>;
+		using ratio = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, std::ratio_divide<typename Base::_RATIO1, typename Base::_RATIO2CONV>, std::ratio_divide<typename Base::_RATIO1, typename Base::_RATIO2>>;
+		/** @endcond */	// END DOXYGEN IGNORE
+
+		/**
+		 * @brief		Value of quotient
+		 * @details		Returns the calculated value of the quotient of `U1` and `U2`, in units
+		 *				of `U1 x U2`.
+		 * @returns		Value of the quotient in the appropriate units.
+		 */
+		static constexpr const unit_t<unit_type> value() noexcept
+		{
+			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
+			return value(UsePi());
+		}
+
+		/** @cond */	// DOXYGEN IGNORE
+		// value if PI isn't involved
+		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
+		{
+			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
+		}
+
+		// value if PI *is* involved
+		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
+		{
+			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
+		}
+		/** @endcond */	// END DOXYGEN IGNORE
+	};
+
+	/**
+	 * @ingroup		CompileTimeUnitManipulators
+	 * @brief		raises unit_value_to a power at compile-time
+	 * @details		The resulting unit will the `unit_type` of `U1` squared
+	 * @tparam		U1	`unit_value_t` to take the exponentiation of.
+	 * @sa			unit_value_t_traits to access information about the properties of the class,
+	 *				such as it's unit type and rational value.
+	 * @note		very similar in concept to `units::math::pow`
+	 */
+	template<class U1, int power>
+	struct unit_value_power : units::detail::unit_value_arithmetic<U1, U1>, units::detail::_unit_value_t<typename units::detail::power_of_unit<power, typename traits::unit_value_t_traits<U1>::unit_type>::type>
+	{
+		/** @cond */	// DOXYGEN IGNORE
+		using Base = units::detail::unit_value_arithmetic<U1, U1>;
+
+		using unit_type = typename units::detail::power_of_unit<power, typename Base::_UNIT1>::type;
+		using ratio = typename units::detail::power_of_ratio<power, typename Base::_RATIO1>::type;
+		using pi_exponent = std::ratio_multiply<std::ratio<power>, typename Base::_UNIT1::pi_exponent_ratio>;
+		/** @endcond */	// END DOXYGEN IGNORE
+
+		/**
+		 * @brief		Value of exponentiation
+		 * @details		Returns the calculated value of the exponentiation of `U1`, in units
+		 *				of `U1^power`.
+		 * @returns		Value of the exponentiation in the appropriate units.
+		 */
+		static constexpr const unit_t<unit_type> value() noexcept
+		{
+			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
+			return value(UsePi());
+		}
+
+		/** @cond */	// DOXYGEN IGNORE
+		// value if PI isn't involved
+		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
+		{
+			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
+		}
+
+		// value if PI *is* involved
+		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
+		{
+			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)pi_exponent::num / pi_exponent::den)));
+		}
+		/** @endcond */	// END DOXYGEN IGNORE	};
+	};
+
+	/**
+	 * @ingroup		CompileTimeUnitManipulators
+	 * @brief		calculates square root of unit_value_t at compile-time
+	 * @details		The resulting unit will the square root `unit_type` of `U1`
+	 * @tparam		U1	`unit_value_t` to take the square root of.
+	 * @sa			unit_value_t_traits to access information about the properties of the class,
+	 *				such as it's unit type and rational value.
+	 * @note		very similar in concept to `units::ratio_sqrt`
+	 */
+	template<class U1, std::intmax_t Eps = 10000000000>
+	struct unit_value_sqrt : units::detail::unit_value_arithmetic<U1, U1>, units::detail::_unit_value_t<square_root<typename traits::unit_value_t_traits<U1>::unit_type, Eps>>
+	{
+		/** @cond */	// DOXYGEN IGNORE
+		using Base = units::detail::unit_value_arithmetic<U1, U1>;
+
+		using unit_type = square_root<typename Base::_UNIT1, Eps>;
+		using ratio = ratio_sqrt<typename Base::_RATIO1, Eps>;
+		using pi_exponent = ratio_sqrt<typename Base::_UNIT1::pi_exponent_ratio, Eps>;
+		/** @endcond */	// END DOXYGEN IGNORE
+
+		/**
+		 * @brief		Value of square root
+		 * @details		Returns the calculated value of the square root of `U1`, in units
+		 *				of `U1^1/2`.
+		 * @returns		Value of the square root in the appropriate units.
+		 */
+		static constexpr const unit_t<unit_type> value() noexcept
+		{
+			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
+			return value(UsePi());
+		}
+
+		/** @cond */	// DOXYGEN IGNORE
+		// value if PI isn't involved
+		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
+		{
+			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
+		}
+
+		// value if PI *is* involved
+		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
+		{
+			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)pi_exponent::num / pi_exponent::den)));
+		}
+		/** @endcond */	// END DOXYGEN IGNORE
+	};
+
+	//----------------------------------
+	//	UNIT-ENABLED CMATH FUNCTIONS
+	//----------------------------------
+
+	/**
+	 * @brief		namespace for unit-enabled versions of the `<cmath>` library
+	 * @details		Includes trigonometric functions, exponential/log functions, rounding functions, etc.
+	 * @sa			See `unit_t` for more information on unit type containers.
+	 */
+	namespace math
+	{
+
+		//----------------------------------
+		//	MIN/MAX FUNCTIONS
+		//----------------------------------
+		// XXX: min/max are defined here instead of math.h to avoid a conflict with
+		// the "_min" user-defined literal in time.h.
+
+		template<class UnitTypeLhs, class UnitTypeRhs>
+		UnitTypeLhs (min)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
+		{
+			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Unit types are not compatible.");
+			UnitTypeLhs r(rhs);
+			return (lhs < r ? lhs : r);
+		}
+
+		template<class UnitTypeLhs, class UnitTypeRhs>
+		UnitTypeLhs (max)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
+		{
+			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Unit types are not compatible.");
+			UnitTypeLhs r(rhs);
+			return (lhs > r ? lhs : r);
+		}
+	}
+}
+
+#ifdef _MSC_VER
+#	if _MSC_VER <= 1800
+#		pragma warning(pop)
+#		undef constexpr
+#		pragma pop_macro("constexpr")
+#		undef noexcept
+#		pragma pop_macro("noexcept")
+#		undef _ALLOW_KEYWORD_MACROS
+#	endif // _MSC_VER < 1800
+#	pragma pop_macro("pascal")
+#endif // _MSC_VER
+
+#if defined(UNIT_HAS_LITERAL_SUPPORT)
+namespace units::literals {}
+using namespace units::literals;
+#endif  // UNIT_HAS_LITERAL_SUPPORT

wpimath/src/main/native/include/units/capacitance.h

diff --git a/wpimath/src/main/native/include/units/capacitance.h b/wpimath/src/main/native/include/units/capacitance.h
new file mode 100644
index 0000000..feaf88c
--- /dev/null
+++ b/wpimath/src/main/native/include/units/capacitance.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::capacitance
+ * @brief namespace for unit types and containers representing capacitance
+ *        values
+ * @details The SI unit for capacitance is `farads`, and the corresponding
+ *          `base_unit` category is `capacitance_unit`.
+ * @anchor capacitanceContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_CAPACITANCE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    capacitance, farad, farads, F,
+    unit<std::ratio<1>, units::category::capacitance_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(capacitance)
+#endif
+
+using namespace capacitance;
+}  // namespace units

wpimath/src/main/native/include/units/charge.h

diff --git a/wpimath/src/main/native/include/units/charge.h b/wpimath/src/main/native/include/units/charge.h
new file mode 100644
index 0000000..42064b0
--- /dev/null
+++ b/wpimath/src/main/native/include/units/charge.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/current.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::charge
+ * @brief namespace for unit types and containers representing charge values
+ * @details The SI unit for charge is `coulombs`, and the corresponding
+ *          `base_unit` category is `charge_unit`.
+ * @anchor chargeContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_CHARGE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(charge, coulomb, coulombs, C,
+                              unit<std::ratio<1>, units::category::charge_unit>)
+UNIT_ADD_WITH_METRIC_PREFIXES(charge, ampere_hour, ampere_hours, Ah,
+                              compound_unit<current::ampere, time::hours>)
+
+UNIT_ADD_CATEGORY_TRAIT(charge)
+#endif
+
+using namespace charge;
+}  // namespace units

wpimath/src/main/native/include/units/concentration.h

diff --git a/wpimath/src/main/native/include/units/concentration.h b/wpimath/src/main/native/include/units/concentration.h
new file mode 100644
index 0000000..b276f82
--- /dev/null
+++ b/wpimath/src/main/native/include/units/concentration.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::concentration
+ * @brief namespace for unit types and containers representing concentration
+ *        values
+ * @details The SI unit for concentration is `parts_per_million`, and the
+ *          corresponding `base_unit` category is `scalar_unit`.
+ * @anchor concentrationContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_CONCENTRATION_UNITS)
+UNIT_ADD(concentration, ppm, parts_per_million, ppm,
+         unit<std::ratio<1, 1000000>, units::category::scalar_unit>)
+UNIT_ADD(concentration, ppb, parts_per_billion, ppb,
+         unit<std::ratio<1, 1000>, parts_per_million>)
+UNIT_ADD(concentration, ppt, parts_per_trillion, ppt,
+         unit<std::ratio<1, 1000>, parts_per_billion>)
+UNIT_ADD(concentration, percent, percent, pct,
+         unit<std::ratio<1, 100>, units::category::scalar_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(concentration)
+#endif
+
+using namespace concentration;
+}  // namespace units

wpimath/src/main/native/include/units/conductance.h

diff --git a/wpimath/src/main/native/include/units/conductance.h b/wpimath/src/main/native/include/units/conductance.h
new file mode 100644
index 0000000..d0508c4
--- /dev/null
+++ b/wpimath/src/main/native/include/units/conductance.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::conductance
+ * @brief namespace for unit types and containers representing conductance
+ *        values
+ * @details The SI unit for conductance is `siemens`, and the corresponding
+ *          `base_unit` category is `conductance_unit`.
+ * @anchor conductanceContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_CONDUCTANCE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    conductance, siemens, siemens, S,
+    unit<std::ratio<1>, units::category::conductance_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(conductance)
+#endif
+
+using namespace conductance;
+}  // namespace units

wpimath/src/main/native/include/units/constants.h

diff --git a/wpimath/src/main/native/include/units/constants.h b/wpimath/src/main/native/include/units/constants.h
new file mode 100644
index 0000000..7d5c49f
--- /dev/null
+++ b/wpimath/src/main/native/include/units/constants.h
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/area.h"
+#include "units/capacitance.h"
+#include "units/charge.h"
+#include "units/current.h"
+#include "units/dimensionless.h"
+#include "units/energy.h"
+#include "units/force.h"
+#include "units/impedance.h"
+#include "units/length.h"
+#include "units/magnetic_field_strength.h"
+#include "units/mass.h"
+#include "units/power.h"
+#include "units/substance.h"
+#include "units/temperature.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+namespace units {
+/**
+ * @brief namespace for physical constants like PI and Avogadro's Number.
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS)
+namespace constants {
+/**
+ * @name Unit Containers
+ * @anchor constantContainers
+ * @{
+ */
+using PI = unit<std::ratio<1>, dimensionless::scalar, std::ratio<1>>;
+
+static constexpr const unit_t<PI> pi(
+    1);  ///< Ratio of a circle's circumference to its diameter.
+static constexpr const velocity::meters_per_second_t c(
+    299792458.0);  ///< Speed of light in vacuum.
+static constexpr const unit_t<
+    compound_unit<cubed<length::meters>, inverse<mass::kilogram>,
+                  inverse<squared<time::seconds>>>>
+    G(6.67408e-11);  ///< Newtonian constant of gravitation.
+static constexpr const unit_t<compound_unit<energy::joule, time::seconds>> h(
+    6.626070040e-34);  ///< Planck constant.
+static constexpr const unit_t<
+    compound_unit<force::newtons, inverse<squared<current::ampere>>>>
+    mu0(pi * 4.0e-7 * force::newton_t(1) /
+        units::math::cpow<2>(current::ampere_t(1)));  ///< vacuum permeability.
+static constexpr const unit_t<
+    compound_unit<capacitance::farad, inverse<length::meter>>>
+    epsilon0(1.0 / (mu0 * math::cpow<2>(c)));  ///< vacuum permitivity.
+static constexpr const impedance::ohm_t Z0(
+    mu0* c);  ///< characteristic impedance of vacuum.
+static constexpr const unit_t<compound_unit<force::newtons, area::square_meter,
+                                            inverse<squared<charge::coulomb>>>>
+    k_e(1.0 / (4 * pi * epsilon0));  ///< Coulomb's constant.
+static constexpr const charge::coulomb_t e(
+    1.6021766208e-19);  ///< elementary charge.
+static constexpr const mass::kilogram_t m_e(
+    9.10938356e-31);  ///< electron mass.
+static constexpr const mass::kilogram_t m_p(1.672621898e-27);  ///< proton mass.
+static constexpr const unit_t<
+    compound_unit<energy::joules, inverse<magnetic_field_strength::tesla>>>
+mu_B(e* h / (4 * pi * m_e));  ///< Bohr magneton.
+static constexpr const unit_t<inverse<substance::mol>> N_A(
+    6.022140857e23);  ///< Avagadro's Number.
+static constexpr const unit_t<compound_unit<
+    energy::joules, inverse<temperature::kelvin>, inverse<substance::moles>>>
+    R(8.3144598);  ///< Gas constant.
+static constexpr const unit_t<
+    compound_unit<energy::joules, inverse<temperature::kelvin>>>
+    k_B(R / N_A);  ///< Boltzmann constant.
+static constexpr const unit_t<
+    compound_unit<charge::coulomb, inverse<substance::mol>>>
+F(N_A* e);  ///< Faraday constant.
+static constexpr const unit_t<
+    compound_unit<power::watts, inverse<area::square_meters>,
+                  inverse<squared<squared<temperature::kelvin>>>>>
+    sigma((2 * math::cpow<5>(pi) * math::cpow<4>(R)) /
+          (15 * math::cpow<3>(h) * math::cpow<2>(c) *
+           math::cpow<4>(N_A)));  ///< Stefan-Boltzmann constant.
+/** @} */
+}  // namespace constants
+#endif
+}  // namespace units

wpimath/src/main/native/include/units/current.h

diff --git a/wpimath/src/main/native/include/units/current.h b/wpimath/src/main/native/include/units/current.h
new file mode 100644
index 0000000..54a408c
--- /dev/null
+++ b/wpimath/src/main/native/include/units/current.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::current
+ * @brief namespace for unit types and containers representing current values
+ * @details The SI unit for current is `amperes`, and the corresponding
+ *          `base_unit` category is `current_unit`.
+ * @anchor currentContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_CURRENT_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    current, ampere, amperes, A,
+    unit<std::ratio<1>, units::category::current_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(current)
+#endif
+
+using namespace current;
+}  // namespace units

wpimath/src/main/native/include/units/curvature.h

diff --git a/wpimath/src/main/native/include/units/curvature.h b/wpimath/src/main/native/include/units/curvature.h
new file mode 100644
index 0000000..233ad61
--- /dev/null
+++ b/wpimath/src/main/native/include/units/curvature.h
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/angle.h"
+#include "units/base.h"
+#include "units/length.h"
+
+namespace units {
+using curvature_t = units::unit_t<
+    units::compound_unit<units::radians, units::inverse<units::meters>>>;
+}  // namespace units

wpimath/src/main/native/include/units/data.h

diff --git a/wpimath/src/main/native/include/units/data.h b/wpimath/src/main/native/include/units/data.h
new file mode 100644
index 0000000..386c0c2
--- /dev/null
+++ b/wpimath/src/main/native/include/units/data.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::data
+ * @brief namespace for unit types and containers representing data values
+ * @details The base unit for data is `bytes`, and the corresponding `base_unit`
+ *          category is `data_unit`.
+ * @anchor dataContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_DATA_UNITS)
+UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(
+    data, byte, bytes, B, unit<std::ratio<1>, units::category::data_unit>)
+UNIT_ADD(data, exabyte, exabytes, EB, unit<std::ratio<1000>, petabytes>)
+UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(data, bit, bits, b,
+                                         unit<std::ratio<1, 8>, byte>)
+UNIT_ADD(data, exabit, exabits, Eb, unit<std::ratio<1000>, petabits>)
+
+UNIT_ADD_CATEGORY_TRAIT(data)
+#endif
+
+using namespace data;
+}  // namespace units

wpimath/src/main/native/include/units/data_transfer_rate.h

diff --git a/wpimath/src/main/native/include/units/data_transfer_rate.h b/wpimath/src/main/native/include/units/data_transfer_rate.h
new file mode 100644
index 0000000..67de063
--- /dev/null
+++ b/wpimath/src/main/native/include/units/data_transfer_rate.h
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::data_transfer_rate
+ * @brief namespace for unit types and containers representing data values
+ * @details The base unit for data is `bytes`, and the corresponding `base_unit`
+ *          category is `data_unit`.
+ * @anchor dataContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_DATA_TRANSFER_RATE_UNITS)
+UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(
+    data_transfer_rate, bytes_per_second, bytes_per_second, Bps,
+    unit<std::ratio<1>, units::category::data_transfer_rate_unit>)
+UNIT_ADD(data_transfer_rate, exabytes_per_second, exabytes_per_second, EBps,
+         unit<std::ratio<1000>, petabytes_per_second>)
+UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(
+    data_transfer_rate, bits_per_second, bits_per_second, bps,
+    unit<std::ratio<1, 8>, bytes_per_second>)
+UNIT_ADD(data_transfer_rate, exabits_per_second, exabits_per_second, Ebps,
+         unit<std::ratio<1000>, petabits_per_second>)
+
+UNIT_ADD_CATEGORY_TRAIT(data_transfer_rate)
+#endif
+
+using namespace data_transfer_rate;
+}  // namespace units

wpimath/src/main/native/include/units/density.h

diff --git a/wpimath/src/main/native/include/units/density.h b/wpimath/src/main/native/include/units/density.h
new file mode 100644
index 0000000..2509f49
--- /dev/null
+++ b/wpimath/src/main/native/include/units/density.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/mass.h"
+#include "units/volume.h"
+
+namespace units {
+/**
+ * @namespace units::density
+ * @brief namespace for unit types and containers representing density values
+ * @details The SI unit for density is `kilograms_per_cubic_meter`, and the
+ *          corresponding `base_unit` category is `density_unit`.
+ * @anchor densityContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_DENSITY_UNITS)
+UNIT_ADD(density, kilograms_per_cubic_meter, kilograms_per_cubic_meter,
+         kg_per_cu_m, unit<std::ratio<1>, units::category::density_unit>)
+UNIT_ADD(density, grams_per_milliliter, grams_per_milliliter, g_per_mL,
+         compound_unit<mass::grams, inverse<volume::milliliter>>)
+UNIT_ADD(density, kilograms_per_liter, kilograms_per_liter, kg_per_L,
+         unit<std::ratio<1>,
+              compound_unit<mass::grams, inverse<volume::milliliter>>>)
+UNIT_ADD(density, ounces_per_cubic_foot, ounces_per_cubic_foot, oz_per_cu_ft,
+         compound_unit<mass::ounces, inverse<volume::cubic_foot>>)
+UNIT_ADD(density, ounces_per_cubic_inch, ounces_per_cubic_inch, oz_per_cu_in,
+         compound_unit<mass::ounces, inverse<volume::cubic_inch>>)
+UNIT_ADD(density, ounces_per_gallon, ounces_per_gallon, oz_per_gal,
+         compound_unit<mass::ounces, inverse<volume::gallon>>)
+UNIT_ADD(density, pounds_per_cubic_foot, pounds_per_cubic_foot, lb_per_cu_ft,
+         compound_unit<mass::pounds, inverse<volume::cubic_foot>>)
+UNIT_ADD(density, pounds_per_cubic_inch, pounds_per_cubic_inch, lb_per_cu_in,
+         compound_unit<mass::pounds, inverse<volume::cubic_inch>>)
+UNIT_ADD(density, pounds_per_gallon, pounds_per_gallon, lb_per_gal,
+         compound_unit<mass::pounds, inverse<volume::gallon>>)
+UNIT_ADD(density, slugs_per_cubic_foot, slugs_per_cubic_foot, slug_per_cu_ft,
+         compound_unit<mass::slugs, inverse<volume::cubic_foot>>)
+
+UNIT_ADD_CATEGORY_TRAIT(density)
+#endif
+
+using namespace density;
+}  // namespace units

wpimath/src/main/native/include/units/dimensionless.h

diff --git a/wpimath/src/main/native/include/units/dimensionless.h b/wpimath/src/main/native/include/units/dimensionless.h
new file mode 100644
index 0000000..64f75ba
--- /dev/null
+++ b/wpimath/src/main/native/include/units/dimensionless.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+using dimensionless::dB_t;
+using dimensionless::dimensionless_t;
+using dimensionless::scalar;
+using dimensionless::scalar_t;
+}  // namespace units

wpimath/src/main/native/include/units/energy.h

diff --git a/wpimath/src/main/native/include/units/energy.h b/wpimath/src/main/native/include/units/energy.h
new file mode 100644
index 0000000..c206e5d
--- /dev/null
+++ b/wpimath/src/main/native/include/units/energy.h
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::energy
+ * @brief namespace for unit types and containers representing energy values
+ * @details The SI unit for energy is `joules`, and the corresponding
+ *          `base_unit` category is `energy_unit`.
+ * @anchor energyContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_ENERGY_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(energy, joule, joules, J,
+                              unit<std::ratio<1>, units::category::energy_unit>)
+UNIT_ADD_WITH_METRIC_PREFIXES(energy, calorie, calories, cal,
+                              unit<std::ratio<4184, 1000>, joules>)
+UNIT_ADD(energy, kilowatt_hour, kilowatt_hours, kWh,
+         unit<std::ratio<36, 10>, megajoules>)
+UNIT_ADD(energy, watt_hour, watt_hours, Wh,
+         unit<std::ratio<1, 1000>, kilowatt_hours>)
+UNIT_ADD(energy, british_thermal_unit, british_thermal_units, BTU,
+         unit<std::ratio<105505585262, 100000000>, joules>)
+UNIT_ADD(energy, british_thermal_unit_iso, british_thermal_units_iso, BTU_iso,
+         unit<std::ratio<1055056, 1000>, joules>)
+UNIT_ADD(energy, british_thermal_unit_59, british_thermal_units_59, BTU59,
+         unit<std::ratio<1054804, 1000>, joules>)
+UNIT_ADD(energy, therm, therms, thm,
+         unit<std::ratio<100000>, british_thermal_units_59>)
+UNIT_ADD(energy, foot_pound, foot_pounds, ftlbf,
+         unit<std::ratio<13558179483314004, 10000000000000000>, joules>)
+
+UNIT_ADD_CATEGORY_TRAIT(energy)
+#endif
+
+using namespace energy;
+}  // namespace units

wpimath/src/main/native/include/units/force.h

diff --git a/wpimath/src/main/native/include/units/force.h b/wpimath/src/main/native/include/units/force.h
new file mode 100644
index 0000000..9813958
--- /dev/null
+++ b/wpimath/src/main/native/include/units/force.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/acceleration.h"
+#include "units/base.h"
+#include "units/length.h"
+#include "units/mass.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::force
+ * @brief namespace for unit types and containers representing force values
+ * @details The SI unit for force is `newtons`, and the corresponding
+ *          `base_unit` category is `force_unit`.
+ * @anchor forceContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_FORCE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(force, newton, newtons, N,
+                              unit<std::ratio<1>, units::category::force_unit>)
+UNIT_ADD(
+    force, pound, pounds, lbf,
+    compound_unit<mass::slug, length::foot, inverse<squared<time::seconds>>>)
+UNIT_ADD(force, dyne, dynes, dyn, unit<std::ratio<1, 100000>, newtons>)
+UNIT_ADD(force, kilopond, kiloponds, kp,
+         compound_unit<acceleration::standard_gravity, mass::kilograms>)
+UNIT_ADD(
+    force, poundal, poundals, pdl,
+    compound_unit<mass::pound, length::foot, inverse<squared<time::seconds>>>)
+
+UNIT_ADD_CATEGORY_TRAIT(force)
+#endif
+
+using force::newton_t;
+using force::newtons;
+}  // namespace units

wpimath/src/main/native/include/units/frequency.h

diff --git a/wpimath/src/main/native/include/units/frequency.h b/wpimath/src/main/native/include/units/frequency.h
new file mode 100644
index 0000000..f030329
--- /dev/null
+++ b/wpimath/src/main/native/include/units/frequency.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::frequency
+ * @brief namespace for unit types and containers representing frequency values
+ * @details The SI unit for frequency is `hertz`, and the corresponding
+ *          `base_unit` category is `frequency_unit`.
+ * @anchor frequencyContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_FREQUENCY_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    frequency, hertz, hertz, Hz,
+    unit<std::ratio<1>, units::category::frequency_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(frequency)
+#endif
+
+using namespace frequency;
+}  // namespace units

wpimath/src/main/native/include/units/illuminance.h

diff --git a/wpimath/src/main/native/include/units/illuminance.h b/wpimath/src/main/native/include/units/illuminance.h
new file mode 100644
index 0000000..976f6b5
--- /dev/null
+++ b/wpimath/src/main/native/include/units/illuminance.h
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/length.h"
+#include "units/luminous_flux.h"
+
+namespace units {
+/**
+ * @namespace units::illuminance
+ * @brief namespace for unit types and containers representing illuminance
+ *        values
+ * @details The SI unit for illuminance is `luxes`, and the corresponding
+ *          `base_unit` category is `illuminance_unit`.
+ * @anchor illuminanceContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_ILLUMINANCE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    illuminance, lux, luxes, lx,
+    unit<std::ratio<1>, units::category::illuminance_unit>)
+UNIT_ADD(illuminance, footcandle, footcandles, fc,
+         compound_unit<luminous_flux::lumen, inverse<squared<length::foot>>>)
+UNIT_ADD(illuminance, lumens_per_square_inch, lumens_per_square_inch,
+         lm_per_in_sq,
+         compound_unit<luminous_flux::lumen, inverse<squared<length::inch>>>)
+UNIT_ADD(
+    illuminance, phot, phots, ph,
+    compound_unit<luminous_flux::lumens, inverse<squared<length::centimeter>>>)
+
+UNIT_ADD_CATEGORY_TRAIT(illuminance)
+#endif
+
+using namespace illuminance;
+}  // namespace units

wpimath/src/main/native/include/units/impedance.h

diff --git a/wpimath/src/main/native/include/units/impedance.h b/wpimath/src/main/native/include/units/impedance.h
new file mode 100644
index 0000000..b4b92ad
--- /dev/null
+++ b/wpimath/src/main/native/include/units/impedance.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::impedance
+ * @brief namespace for unit types and containers representing impedance values
+ * @details The SI unit for impedance is `ohms`, and the corresponding
+ *          `base_unit` category is `impedance_unit`.
+ * @anchor impedanceContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_IMPEDANCE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    impedance, ohm, ohms, Ohm,
+    unit<std::ratio<1>, units::category::impedance_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(impedance)
+#endif
+
+using namespace impedance;
+}  // namespace units

wpimath/src/main/native/include/units/inductance.h

diff --git a/wpimath/src/main/native/include/units/inductance.h b/wpimath/src/main/native/include/units/inductance.h
new file mode 100644
index 0000000..6a5be7f
--- /dev/null
+++ b/wpimath/src/main/native/include/units/inductance.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::inductance
+ * @brief namespace for unit types and containers representing inductance values
+ * @details The SI unit for inductance is `henrys`, and the corresponding
+ *          `base_unit` category is `inductance_unit`.
+ * @anchor inductanceContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_INDUCTANCE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    inductance, henry, henries, H,
+    unit<std::ratio<1>, units::category::inductance_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(inductance)
+#endif
+
+using namespace inductance;
+}  // namespace units

wpimath/src/main/native/include/units/length.h

diff --git a/wpimath/src/main/native/include/units/length.h b/wpimath/src/main/native/include/units/length.h
new file mode 100644
index 0000000..637797d
--- /dev/null
+++ b/wpimath/src/main/native/include/units/length.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::length
+ * @brief namespace for unit types and containers representing length values
+ * @details The SI unit for length is `meters`, and the corresponding
+ *          `base_unit` category is `length_unit`.
+ * @anchor lengthContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_LENGTH_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(length, meter, meters, m,
+                              unit<std::ratio<1>, units::category::length_unit>)
+UNIT_ADD(length, foot, feet, ft, unit<std::ratio<381, 1250>, meters>)
+UNIT_ADD(length, mil, mils, mil, unit<std::ratio<1000>, feet>)
+UNIT_ADD(length, inch, inches, in, unit<std::ratio<1, 12>, feet>)
+UNIT_ADD(length, mile, miles, mi, unit<std::ratio<5280>, feet>)
+UNIT_ADD(length, nauticalMile, nauticalMiles, nmi,
+         unit<std::ratio<1852>, meters>)
+UNIT_ADD(length, astronicalUnit, astronicalUnits, au,
+         unit<std::ratio<149597870700>, meters>)
+UNIT_ADD(length, lightyear, lightyears, ly,
+         unit<std::ratio<9460730472580800>, meters>)
+UNIT_ADD(length, parsec, parsecs, pc,
+         unit<std::ratio<648000>, astronicalUnits, std::ratio<-1>>)
+UNIT_ADD(length, angstrom, angstroms, angstrom,
+         unit<std::ratio<1, 10>, nanometers>)
+UNIT_ADD(length, cubit, cubits, cbt, unit<std::ratio<18>, inches>)
+UNIT_ADD(length, fathom, fathoms, ftm, unit<std::ratio<6>, feet>)
+UNIT_ADD(length, chain, chains, ch, unit<std::ratio<66>, feet>)
+UNIT_ADD(length, furlong, furlongs, fur, unit<std::ratio<10>, chains>)
+UNIT_ADD(length, hand, hands, hand, unit<std::ratio<4>, inches>)
+UNIT_ADD(length, league, leagues, lea, unit<std::ratio<3>, miles>)
+UNIT_ADD(length, nauticalLeague, nauticalLeagues, nl,
+         unit<std::ratio<3>, nauticalMiles>)
+UNIT_ADD(length, yard, yards, yd, unit<std::ratio<3>, feet>)
+
+UNIT_ADD_CATEGORY_TRAIT(length)
+#endif
+
+using namespace length;
+}  // namespace units

wpimath/src/main/native/include/units/luminous_flux.h

diff --git a/wpimath/src/main/native/include/units/luminous_flux.h b/wpimath/src/main/native/include/units/luminous_flux.h
new file mode 100644
index 0000000..ca7a079
--- /dev/null
+++ b/wpimath/src/main/native/include/units/luminous_flux.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::luminous_flux
+ * @brief namespace for unit types and containers representing luminous_flux
+ *        values
+ * @details The SI unit for luminous_flux is `lumens`, and the corresponding
+ *          `base_unit` category is `luminous_flux_unit`.
+ * @anchor luminousFluxContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_LUMINOUS_FLUX_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    luminous_flux, lumen, lumens, lm,
+    unit<std::ratio<1>, units::category::luminous_flux_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(luminous_flux)
+#endif
+
+using namespace luminous_flux;
+}  // namespace units

wpimath/src/main/native/include/units/luminous_intensity.h

diff --git a/wpimath/src/main/native/include/units/luminous_intensity.h b/wpimath/src/main/native/include/units/luminous_intensity.h
new file mode 100644
index 0000000..f907d2e
--- /dev/null
+++ b/wpimath/src/main/native/include/units/luminous_intensity.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::luminous_intensity
+ * @brief namespace for unit types and containers representing
+ *        luminous_intensity values
+ * @details The SI unit for luminous_intensity is `candelas`, and the
+ *          corresponding `base_unit` category is `luminous_intensity_unit`.
+ * @anchor luminousIntensityContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_LUMINOUS_INTENSITY_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    luminous_intensity, candela, candelas, cd,
+    unit<std::ratio<1>, units::category::luminous_intensity_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(luminous_intensity)
+#endif
+
+using namespace luminous_intensity;
+}  // namespace units

wpimath/src/main/native/include/units/magnetic_field_strength.h

diff --git a/wpimath/src/main/native/include/units/magnetic_field_strength.h b/wpimath/src/main/native/include/units/magnetic_field_strength.h
new file mode 100644
index 0000000..e7a7086
--- /dev/null
+++ b/wpimath/src/main/native/include/units/magnetic_field_strength.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/length.h"
+#include "units/magnetic_flux.h"
+
+namespace units {
+/**
+ * @namespace units::magnetic_field_strength
+ * @brief namespace for unit types and containers representing
+ *        magnetic_field_strength values
+ * @details The SI unit for magnetic_field_strength is `teslas`, and the
+ *          corresponding `base_unit` category is
+ *          `magnetic_field_strength_unit`.
+ * @anchor magneticFieldStrengthContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+// Unfortunately `_T` is a WINAPI macro, so we have to use `_Te` as the tesla
+// abbreviation.
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_MAGNETIC_FIELD_STRENGTH_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    magnetic_field_strength, tesla, teslas, Te,
+    unit<std::ratio<1>, units::category::magnetic_field_strength_unit>)
+UNIT_ADD(
+    magnetic_field_strength, gauss, gauss, G,
+    compound_unit<magnetic_flux::maxwell, inverse<squared<length::centimeter>>>)
+
+UNIT_ADD_CATEGORY_TRAIT(magnetic_field_strength)
+#endif
+
+using namespace magnetic_field_strength;
+}  // namespace units

wpimath/src/main/native/include/units/magnetic_flux.h

diff --git a/wpimath/src/main/native/include/units/magnetic_flux.h b/wpimath/src/main/native/include/units/magnetic_flux.h
new file mode 100644
index 0000000..739b9e7
--- /dev/null
+++ b/wpimath/src/main/native/include/units/magnetic_flux.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::magnetic_flux
+ * @brief namespace for unit types and containers representing magnetic_flux
+ *        values
+ * @details The SI unit for magnetic_flux is `webers`, and the corresponding
+ *          `base_unit` category is `magnetic_flux_unit`.
+ * @anchor magneticFluxContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_MAGNETIC_FLUX_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    magnetic_flux, weber, webers, Wb,
+    unit<std::ratio<1>, units::category::magnetic_flux_unit>)
+UNIT_ADD(magnetic_flux, maxwell, maxwells, Mx,
+         unit<std::ratio<1, 100000000>, webers>)
+
+UNIT_ADD_CATEGORY_TRAIT(magnetic_flux)
+#endif
+
+using namespace magnetic_flux;
+}  // namespace units

wpimath/src/main/native/include/units/mass.h

diff --git a/wpimath/src/main/native/include/units/mass.h b/wpimath/src/main/native/include/units/mass.h
new file mode 100644
index 0000000..21fa3b5
--- /dev/null
+++ b/wpimath/src/main/native/include/units/mass.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::mass
+ * @brief namespace for unit types and containers representing mass values
+ * @details The SI unit for mass is `kilograms`, and the corresponding
+ *          `base_unit` category is `mass_unit`.
+ * @anchor massContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_MASS_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    mass, gram, grams, g, unit<std::ratio<1, 1000>, units::category::mass_unit>)
+UNIT_ADD(mass, metric_ton, metric_tons, t, unit<std::ratio<1000>, kilograms>)
+UNIT_ADD(mass, pound, pounds, lb,
+         unit<std::ratio<45359237, 100000000>, kilograms>)
+UNIT_ADD(mass, long_ton, long_tons, ln_t, unit<std::ratio<2240>, pounds>)
+UNIT_ADD(mass, short_ton, short_tons, sh_t, unit<std::ratio<2000>, pounds>)
+UNIT_ADD(mass, stone, stone, st, unit<std::ratio<14>, pounds>)
+UNIT_ADD(mass, ounce, ounces, oz, unit<std::ratio<1, 16>, pounds>)
+UNIT_ADD(mass, carat, carats, ct, unit<std::ratio<200>, milligrams>)
+UNIT_ADD(mass, slug, slugs, slug,
+         unit<std::ratio<145939029, 10000000>, kilograms>)
+
+UNIT_ADD_CATEGORY_TRAIT(mass)
+#endif
+
+using namespace mass;
+}  // namespace units

wpimath/src/main/native/include/units/math.h

diff --git a/wpimath/src/main/native/include/units/math.h b/wpimath/src/main/native/include/units/math.h
new file mode 100644
index 0000000..ccb3a62
--- /dev/null
+++ b/wpimath/src/main/native/include/units/math.h
@@ -0,0 +1,779 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "units/angle.h"
+#include "units/base.h"
+#include "units/dimensionless.h"
+
+namespace units {
+//----------------------------------
+// UNIT-ENABLED CMATH FUNCTIONS
+//----------------------------------
+
+/**
+ * @brief namespace for unit-enabled versions of the `<cmath>` library
+ * @details Includes trigonometric functions, exponential/log functions,
+ *          rounding functions, etc.
+ * @sa See `unit_t` for more information on unit type containers.
+ */
+namespace math {
+//----------------------------------
+// TRIGONOMETRIC FUNCTIONS
+//----------------------------------
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute cosine
+ * @details The input value can be in any unit of angle, including radians or
+ *          degrees.
+ * @tparam AngleUnit any `unit_t` type of `category::angle_unit`.
+ * @param[in] angle angle to compute the cosine of
+ * @returns Returns the cosine of <i>angle</i>
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class AngleUnit>
+dimensionless::scalar_t cos(const AngleUnit angle) noexcept {
+  static_assert(
+      traits::is_angle_unit<AngleUnit>::value,
+      "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
+  return dimensionless::scalar_t(
+      std::cos(angle.template convert<angle::radian>()()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute sine
+ * @details The input value can be in any unit of angle, including radians or
+ *          degrees.
+ * @tparam AngleUnit  any `unit_t` type of `category::angle_unit`.
+ * @param[in] angle angle to compute the since of
+ * @returns Returns the sine of <i>angle</i>
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class AngleUnit>
+dimensionless::scalar_t sin(const AngleUnit angle) noexcept {
+  static_assert(
+      traits::is_angle_unit<AngleUnit>::value,
+      "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
+  return dimensionless::scalar_t(
+      std::sin(angle.template convert<angle::radian>()()));
+}
+#endif
+/**
+ * @ingroup UnitMath
+ * @brief Compute tangent
+ * @details The input value can be in any unit of angle, including radians or
+ *          degrees.
+ * @tparam AngleUnit  any `unit_t` type of `category::angle_unit`.
+ * @param[in] angle angle to compute the tangent of
+ * @returns Returns the tangent of <i>angle</i>
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class AngleUnit>
+dimensionless::scalar_t tan(const AngleUnit angle) noexcept {
+  static_assert(
+      traits::is_angle_unit<AngleUnit>::value,
+      "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
+  return dimensionless::scalar_t(
+      std::tan(angle.template convert<angle::radian>()()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute arc cosine
+ * @details Returns the principal value of the arc cosine of x, expressed in
+ *          radians.
+ * @param[in] x Value whose arc cosine is computed, in the interval [-1,+1].
+ * @returns Principal arc cosine of x, in the interval [0,pi] radians.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class ScalarUnit>
+angle::radian_t acos(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return angle::radian_t(std::acos(x()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute arc sine
+ * @details Returns the principal value of the arc sine of x, expressed in
+ *          radians.
+ * @param[in] x Value whose arc sine is computed, in the interval [-1,+1].
+ * @returns Principal arc sine of x, in the interval [-pi/2,+pi/2] radians.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class ScalarUnit>
+angle::radian_t asin(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return angle::radian_t(std::asin(x()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute arc tangent
+ * @details Returns the principal value of the arc tangent of x, expressed in
+ *          radians. Notice that because of the sign ambiguity, the function
+ *          cannot determine with certainty in which quadrant the angle falls
+ *          only by its tangent value. See atan2 for an alternative that takes a
+ *          fractional argument instead.
+ * @tparam AngleUnit  any `unit_t` type of `category::angle_unit`.
+ * @param[in] x Value whose arc tangent is computed, in the interval [-1,+1].
+ * @returns Principal arc tangent of x, in the interval [-pi/2,+pi/2] radians.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class ScalarUnit>
+angle::radian_t atan(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return angle::radian_t(std::atan(x()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute arc tangent with two parameters
+ * @details To compute the value, the function takes into account the sign of
+ *          both arguments in order to determine the quadrant.
+ * @param[in] y y-component of the triangle expressed.
+ * @param[in] x x-component of the triangle expressed.
+ * @returns Returns the principal value of the arc tangent of <i>y/x</i>,
+ *          expressed in radians.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class Y, class X>
+angle::radian_t atan2(const Y y, const X x) noexcept {
+  static_assert(traits::is_dimensionless_unit<decltype(y / x)>::value,
+                "The quantity y/x must yield a dimensionless ratio.");
+
+  // X and Y could be different length units, so normalize them
+  return angle::radian_t(
+      std::atan2(y.template convert<
+                     typename units::traits::unit_t_traits<X>::unit_type>()(),
+                 x()));
+}
+#endif
+
+//----------------------------------
+// HYPERBOLIC TRIG FUNCTIONS
+//----------------------------------
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute hyperbolic cosine
+ * @details The input value can be in any unit of angle, including radians or
+ *          degrees.
+ * @tparam AngleUnit any `unit_t` type of `category::angle_unit`.
+ * @param[in] angle angle to compute the hyperbolic cosine of
+ * @returns Returns the hyperbolic cosine of <i>angle</i>
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class AngleUnit>
+dimensionless::scalar_t cosh(const AngleUnit angle) noexcept {
+  static_assert(
+      traits::is_angle_unit<AngleUnit>::value,
+      "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
+  return dimensionless::scalar_t(
+      std::cosh(angle.template convert<angle::radian>()()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute hyperbolic sine
+ * @details The input value can be in any unit of angle, including radians or
+ *          degrees.
+ * @tparam AngleUnit any `unit_t` type of `category::angle_unit`.
+ * @param[in] angle angle to compute the hyperbolic sine of
+ * @returns Returns the hyperbolic sine of <i>angle</i>
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class AngleUnit>
+dimensionless::scalar_t sinh(const AngleUnit angle) noexcept {
+  static_assert(
+      traits::is_angle_unit<AngleUnit>::value,
+      "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
+  return dimensionless::scalar_t(
+      std::sinh(angle.template convert<angle::radian>()()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute hyperbolic tangent
+ * @details The input value can be in any unit of angle, including radians or
+ *          degrees.
+ * @tparam AngleUnit any `unit_t` type of `category::angle_unit`.
+ * @param[in] angle angle to compute the hyperbolic tangent of
+ * @returns Returns the hyperbolic tangent of <i>angle</i>
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class AngleUnit>
+dimensionless::scalar_t tanh(const AngleUnit angle) noexcept {
+  static_assert(
+      traits::is_angle_unit<AngleUnit>::value,
+      "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
+  return dimensionless::scalar_t(
+      std::tanh(angle.template convert<angle::radian>()()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute arc hyperbolic cosine
+ * @details Returns the nonnegative arc hyperbolic cosine of x, expressed in
+ *          radians.
+ * @param[in] x Value whose arc hyperbolic cosine is computed. If the argument
+ *              is less than 1, a domain error occurs.
+ * @returns Nonnegative arc hyperbolic cosine of x, in the interval
+ *          [0,+INFINITY] radians.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class ScalarUnit>
+angle::radian_t acosh(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return angle::radian_t(std::acosh(x()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute arc hyperbolic sine
+ * @details Returns the arc hyperbolic sine of x, expressed in radians.
+ * @param[in] x Value whose arc hyperbolic sine is computed.
+ * @returns Arc hyperbolic sine of x, in radians.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class ScalarUnit>
+angle::radian_t asinh(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return angle::radian_t(std::asinh(x()));
+}
+#endif
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute arc hyperbolic tangent
+ * @details Returns the arc hyperbolic tangent of x, expressed in radians.
+ * @param[in] x Value whose arc hyperbolic tangent is computed, in the interval
+ *              [-1,+1]. If the argument is out of this interval, a domain error
+ *              occurs. For values of -1 and +1, a pole error may occur.
+ * @returns units::angle::radian_t
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
+template <class ScalarUnit>
+angle::radian_t atanh(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return angle::radian_t(std::atanh(x()));
+}
+#endif
+
+//----------------------------------
+// TRANSCENDENTAL FUNCTIONS
+//----------------------------------
+
+// it makes NO SENSE to put dimensioned units into a transcendental function,
+// and if you think it does you are demonstrably wrong.
+// https://en.wikipedia.org/wiki/Transcendental_function#Dimensional_analysis
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute exponential function
+ * @details Returns the base-e exponential function of x, which is e raised to
+ *          the power x: ex.
+ * @param[in] x scalar value of the exponent.
+ * @returns Exponential value of x.
+ *          If the magnitude of the result is too large to be represented by a
+ *          value of the return type, the function returns HUGE_VAL (or
+ *          HUGE_VALF or HUGE_VALL) with the proper sign, and an overflow range
+ *          error occurs.
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t exp(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return dimensionless::scalar_t(std::exp(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute natural logarithm
+ * @details Returns the natural logarithm of x.
+ * @param[in] x scalar value whose logarithm is calculated. If the argument is
+ *              negative, a domain error occurs.
+ * @sa log10 for more common base-10 logarithms
+ * @returns Natural logarithm of x.
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t log(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return dimensionless::scalar_t(std::log(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute common logarithm
+ * @details Returns the common (base-10) logarithm of x.
+ * @param[in] x Value whose logarithm is calculated. If the argument is
+ *              negative, a domain error occurs.
+ * @returns Common logarithm of x.
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t log10(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return dimensionless::scalar_t(std::log10(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Break into fractional and integral parts.
+ * @details The integer part is stored in the object pointed by intpart, and the
+ *          fractional part is returned by the function. Both parts have the
+ *          same sign as x.
+ * @param[in] x scalar value to break into parts.
+ * @param[in] intpart Pointer to an object (of the same type as x) where the
+ *                    integral part is stored with the same sign as x.
+ * @returns The fractional part of x, with the same sign.
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t modf(const ScalarUnit x, ScalarUnit* intpart) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+
+  UNIT_LIB_DEFAULT_TYPE intp;
+  dimensionless::scalar_t fracpart =
+      dimensionless::scalar_t(std::modf(x(), &intp));
+  *intpart = intp;
+  return fracpart;
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute binary exponential function
+ * @details Returns the base-2 exponential function of x, which is 2 raised to
+ *          the power x: 2^x. 2param[in]  x  Value of the exponent.
+ * @returns 2 raised to the power of x.
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t exp2(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return dimensionless::scalar_t(std::exp2(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute exponential minus one
+ * @details Returns e raised to the power x minus one: e^x-1. For small
+ *          magnitude values of x, expm1 may be more accurate than exp(x)-1.
+ * @param[in] x Value of the exponent.
+ * @returns e raised to the power of x, minus one.
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t expm1(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return dimensionless::scalar_t(std::expm1(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute logarithm plus one
+ * @details Returns the natural logarithm of one plus x. For small magnitude
+ *          values of x, logp1 may be more accurate than log(1+x).
+ * @param[in] x Value whose logarithm is calculated. If the argument is less
+ *              than -1, a domain error occurs.
+ * @returns The natural logarithm of (1+x).
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t log1p(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return dimensionless::scalar_t(std::log1p(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute binary logarithm
+ * @details Returns the binary (base-2) logarithm of x.
+ * @param[in] x Value whose logarithm is calculated. If the argument is
+ *              negative, a domain error occurs.
+ * @returns The binary logarithm of x: log2x.
+ */
+template <class ScalarUnit>
+dimensionless::scalar_t log2(const ScalarUnit x) noexcept {
+  static_assert(
+      traits::is_dimensionless_unit<ScalarUnit>::value,
+      "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
+  return dimensionless::scalar_t(std::log2(x()));
+}
+
+//----------------------------------
+// POWER FUNCTIONS
+//----------------------------------
+
+/* pow is implemented earlier in the library since a lot of the unit definitions
+ * depend on it */
+
+/**
+ * @ingroup UnitMath
+ * @brief computes the square root of <i>value</i>
+ * @details Only implemented for linear_scale units.
+ * @param[in] value `unit_t` derived type to compute the square root of.
+ * @returns new unit_t, whose units are the square root of value's.
+ *          E.g. if values had units of `square_meter`, then the return type
+ *          will have units of `meter`.
+ * @note `sqrt` provides a _rational approximation_ of the square root of
+ *       <i>value</i>. In some cases, _both_ the returned value _and_ conversion
+ *       factor of the returned unit type may have errors no larger than
+ *       `1e-10`.
+ */
+template <
+    class UnitType,
+    std::enable_if_t<units::traits::has_linear_scale<UnitType>::value, int> = 0>
+inline auto sqrt(const UnitType& value) noexcept -> unit_t<
+    square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
+    typename units::traits::unit_t_traits<UnitType>::underlying_type,
+    linear_scale> {
+  return unit_t<
+      square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>,
+      typename units::traits::unit_t_traits<UnitType>::underlying_type,
+      linear_scale>(std::sqrt(value()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Computes the square root of the sum-of-squares of x and y.
+ * @details Only implemented for linear_scale units.
+ * @param[in] x unit_t type value
+ * @param[in] y unit_t type value
+ * @returns square root of the sum-of-squares of x and y in the same units as x.
+ */
+template <class UnitTypeLhs, class UnitTypeRhs,
+          std::enable_if_t<
+              units::traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value,
+              int> = 0>
+inline UnitTypeLhs hypot(const UnitTypeLhs& x, const UnitTypeRhs& y) {
+  static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value,
+                "Parameters of hypot() function are not compatible units.");
+  return UnitTypeLhs(std::hypot(
+      x(),
+      y.template convert<
+          typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
+}
+
+//----------------------------------
+// ROUNDING FUNCTIONS
+//----------------------------------
+
+/**
+ * @ingroup UnitMath
+ * @brief Round up value
+ * @details Rounds x upward, returning the smallest integral value that is not
+ *          less than x.
+ * @param[in] x Unit value to round up.
+ * @returns The smallest integral value that is not less than x.
+ */
+template <class UnitType,
+          class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
+UnitType ceil(const UnitType x) noexcept {
+  return UnitType(std::ceil(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Round down value
+ * @details Rounds x downward, returning the largest integral value that is not
+ *          greater than x.
+ * @param[in] x Unit value to round down.
+ * @returns The value of x rounded downward.
+ */
+template <class UnitType,
+          class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
+UnitType floor(const UnitType x) noexcept {
+  return UnitType(std::floor(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute remainder of division
+ * @details Returns the floating-point remainder of numer/denom (rounded towards
+ *          zero).
+ * @param[in] numer Value of the quotient numerator.
+ * @param[in] denom Value of the quotient denominator.
+ * @returns The remainder of dividing the arguments.
+ */
+template <class UnitTypeLhs, class UnitTypeRhs,
+          class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
+                                   traits::is_unit_t<UnitTypeRhs>::value>>
+UnitTypeLhs fmod(const UnitTypeLhs numer, const UnitTypeRhs denom) noexcept {
+  static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value,
+                "Parameters of fmod() function are not compatible units.");
+  return UnitTypeLhs(std::fmod(
+      numer(),
+      denom.template convert<
+          typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Truncate value
+ * @details Rounds x toward zero, returning the nearest integral value that is
+ *          not larger in magnitude than x. Effectively rounds towards 0.
+ * @param[in] x Value to truncate
+ * @returns The nearest integral value that is not larger in magnitude than x.
+ */
+template <class UnitType,
+          class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
+UnitType trunc(const UnitType x) noexcept {
+  return UnitType(std::trunc(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Round to nearest
+ * @details Returns the integral value that is nearest to x, with halfway cases
+ *          rounded away from zero.
+ * @param[in] x value to round.
+ * @returns The value of x rounded to the nearest integral.
+ */
+template <class UnitType,
+          class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
+UnitType round(const UnitType x) noexcept {
+  return UnitType(std::round(x()));
+}
+
+//----------------------------------
+// FLOATING POINT MANIPULATION
+//----------------------------------
+
+/**
+ * @ingroup UnitMath
+ * @brief Copy sign
+ * @details Returns a value with the magnitude and dimension of x, and the sign
+ *          of y. Values x and y do not have to be compatible units.
+ * @param[in] x Value with the magnitude of the resulting value.
+ * @param[in] y Value with the sign of the resulting value.
+ * @returns value with the magnitude and dimension of x, and the sign of y.
+ */
+template <class UnitTypeLhs, class UnitTypeRhs,
+          class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
+                                   traits::is_unit_t<UnitTypeRhs>::value>>
+UnitTypeLhs copysign(const UnitTypeLhs x, const UnitTypeRhs y) noexcept {
+  return UnitTypeLhs(std::copysign(
+      x(), y()));  // no need for conversion to get the correct sign.
+}
+
+/// Overload to copy the sign from a raw double
+template <class UnitTypeLhs,
+          class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value>>
+UnitTypeLhs copysign(const UnitTypeLhs x,
+                     const UNIT_LIB_DEFAULT_TYPE y) noexcept {
+  return UnitTypeLhs(std::copysign(x(), y));
+}
+
+//----------------------------------
+// MIN / MAX / DIFFERENCE
+//----------------------------------
+
+/**
+ * @ingroup UnitMath
+ * @brief Positive difference
+ * @details The function returns x-y if x>y, and zero otherwise, in the same
+ *          units as x. Values x and y do not have to be the same type of units,
+ *          but they do have to be compatible.
+ * @param[in] x Values whose difference is calculated.
+ * @param[in] y Values whose difference is calculated.
+ * @returns The positive difference between x and y.
+ */
+template <class UnitTypeLhs, class UnitTypeRhs,
+          class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
+                                   traits::is_unit_t<UnitTypeRhs>::value>>
+UnitTypeLhs fdim(const UnitTypeLhs x, const UnitTypeRhs y) noexcept {
+  static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value,
+                "Parameters of fdim() function are not compatible units.");
+  return UnitTypeLhs(std::fdim(
+      x(),
+      y.template convert<
+          typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Maximum value
+ * @details Returns the larger of its arguments: either x or y, in the same
+ *          units as x. Values x and y do not have to be the same type of units,
+ *          but they do have to be compatible.
+ * @param[in] x Values among which the function selects a maximum.
+ * @param[in] y Values among which the function selects a maximum.
+ * @returns The maximum numeric value of its arguments.
+ */
+template <class UnitTypeLhs, class UnitTypeRhs,
+          class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
+                                   traits::is_unit_t<UnitTypeRhs>::value>>
+UnitTypeLhs fmax(const UnitTypeLhs x, const UnitTypeRhs y) noexcept {
+  static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value,
+                "Parameters of fmax() function are not compatible units.");
+  return UnitTypeLhs(std::fmax(
+      x(),
+      y.template convert<
+          typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Minimum value
+ * @details Returns the smaller of its arguments: either x or y, in the same
+ *          units as x. If one of the arguments in a NaN, the other is returned.
+ *          Values x and y do not have to be the same type of units, but they do
+ *          have to be compatible.
+ * @param[in] x Values among which the function selects a minimum.
+ * @param[in] y Values among which the function selects a minimum.
+ * @returns The minimum numeric value of its arguments.
+ */
+template <class UnitTypeLhs, class UnitTypeRhs,
+          class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
+                                   traits::is_unit_t<UnitTypeRhs>::value>>
+UnitTypeLhs fmin(const UnitTypeLhs x, const UnitTypeRhs y) noexcept {
+  static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value,
+                "Parameters of fmin() function are not compatible units.");
+  return UnitTypeLhs(std::fmin(
+      x(),
+      y.template convert<
+          typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
+}
+
+//----------------------------------
+// OTHER FUNCTIONS
+//----------------------------------
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute absolute value
+ * @details Returns the absolute value of x, i.e. |x|.
+ * @param[in] x Value whose absolute value is returned.
+ * @returns The absolute value of x.
+ */
+template <class UnitType,
+          class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
+UnitType fabs(const UnitType x) noexcept {
+  return UnitType(std::fabs(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Compute absolute value
+ * @details Returns the absolute value of x, i.e. |x|.
+ * @param[in] x Value whose absolute value is returned.
+ * @returns The absolute value of x.
+ */
+template <class UnitType,
+          class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
+UnitType abs(const UnitType x) noexcept {
+  return UnitType(std::fabs(x()));
+}
+
+/**
+ * @ingroup UnitMath
+ * @brief Multiply-add
+ * @details Returns x*y+z. The function computes the result without losing
+ *          precision in any intermediate result. The resulting unit type is a
+ *          compound unit of x* y.
+ * @param[in] x Values to be multiplied.
+ * @param[in] y Values to be multiplied.
+ * @param[in] z Value to be added.
+ * @returns The result of x*y+z
+ */
+template <class UnitTypeLhs, class UnitMultiply, class UnitAdd,
+          class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value &&
+                                   traits::is_unit_t<UnitMultiply>::value &&
+                                   traits::is_unit_t<UnitAdd>::value>>
+auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept
+    -> decltype(x * y) {
+  using resultType = decltype(x * y);
+  static_assert(
+      traits::is_convertible_unit_t<
+          compound_unit<
+              typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type,
+              typename units::traits::unit_t_traits<UnitMultiply>::unit_type>,
+          typename units::traits::unit_t_traits<UnitAdd>::unit_type>::value,
+      "Unit types are not compatible.");
+  return resultType(std::fma(x(), y(), resultType(z)()));
+}
+
+/**
+ * Constrains theta to within the range (-pi, pi].
+ *
+ * @param theta Angle to normalize.
+ */
+constexpr units::radian_t NormalizeAngle(units::radian_t theta) {
+  units::radian_t pi(wpi::math::pi);
+
+  // Constrain theta to within (-3pi, pi)
+  const int n_pi_pos = (theta + pi) / 2.0 / pi;
+  theta = theta - units::radian_t{n_pi_pos * 2.0 * wpi::math::pi};
+
+  // Cut off the bottom half of the above range to constrain within
+  // (-pi, pi]
+  const int n_pi_neg = (theta - pi) / 2.0 / pi;
+  theta = theta - units::radian_t{n_pi_neg * 2.0 * wpi::math::pi};
+
+  return theta;
+}
+}  // namespace math
+}  // namespace units

wpimath/src/main/native/include/units/moment_of_inertia.h

diff --git a/wpimath/src/main/native/include/units/moment_of_inertia.h b/wpimath/src/main/native/include/units/moment_of_inertia.h
new file mode 100644
index 0000000..938a635
--- /dev/null
+++ b/wpimath/src/main/native/include/units/moment_of_inertia.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "units/area.h"
+#include "units/base.h"
+#include "units/mass.h"
+
+namespace units {
+UNIT_ADD(moment_of_inertia, kilogram_square_meter, kilogram_square_meters,
+         kg_sq_m, compound_unit<mass::kilograms, area::square_meters>)
+
+using namespace moment_of_inertia;
+}  // namespace units

wpimath/src/main/native/include/units/power.h

diff --git a/wpimath/src/main/native/include/units/power.h b/wpimath/src/main/native/include/units/power.h
new file mode 100644
index 0000000..d1a9504
--- /dev/null
+++ b/wpimath/src/main/native/include/units/power.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::power
+ * @brief namespace for unit types and containers representing power values
+ * @details The SI unit for power is `watts`, and the corresponding `base_unit`
+ *          category is `power_unit`.
+ * @anchor powerContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_POWER_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(power, watt, watts, W,
+                              unit<std::ratio<1>, units::category::power_unit>)
+UNIT_ADD(power, horsepower, horsepower, hp, unit<std::ratio<7457, 10>, watts>)
+UNIT_ADD_DECIBEL(power, watt, dBW)
+UNIT_ADD_DECIBEL(power, milliwatt, dBm)
+
+UNIT_ADD_CATEGORY_TRAIT(power)
+#endif
+
+using namespace power;
+}  // namespace units

wpimath/src/main/native/include/units/pressure.h

diff --git a/wpimath/src/main/native/include/units/pressure.h b/wpimath/src/main/native/include/units/pressure.h
new file mode 100644
index 0000000..c14bae1
--- /dev/null
+++ b/wpimath/src/main/native/include/units/pressure.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/force.h"
+#include "units/length.h"
+
+namespace units {
+/**
+ * @namespace units::pressure
+ * @brief namespace for unit types and containers representing pressure values
+ * @details The SI unit for pressure is `pascals`, and the corresponding
+ *          `base_unit` category is `pressure_unit`.
+ * @anchor pressureContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_PRESSURE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    pressure, pascal, pascals, Pa,
+    unit<std::ratio<1>, units::category::pressure_unit>)
+UNIT_ADD(pressure, bar, bars, bar, unit<std::ratio<100>, kilo<pascals>>)
+UNIT_ADD(pressure, mbar, mbars, mbar, unit<std::ratio<1>, milli<bar>>)
+UNIT_ADD(pressure, atmosphere, atmospheres, atm,
+         unit<std::ratio<101325>, pascals>)
+UNIT_ADD(pressure, pounds_per_square_inch, pounds_per_square_inch, psi,
+         compound_unit<force::pounds, inverse<squared<length::inch>>>)
+UNIT_ADD(pressure, torr, torrs, torr, unit<std::ratio<1, 760>, atmospheres>)
+
+UNIT_ADD_CATEGORY_TRAIT(pressure)
+#endif
+
+using namespace pressure;
+}  // namespace units

wpimath/src/main/native/include/units/radiation.h

diff --git a/wpimath/src/main/native/include/units/radiation.h b/wpimath/src/main/native/include/units/radiation.h
new file mode 100644
index 0000000..b631336
--- /dev/null
+++ b/wpimath/src/main/native/include/units/radiation.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+//--------------------------------------------------------------------------------------------------
+//
+// Copyright (c) 2016 Nic Holthaus
+//
+//--------------------------------------------------------------------------------------------------
+
+#pragma once
+
+#include "units/base.h"
+#include "units/energy.h"
+#include "units/frequency.h"
+
+namespace units {
+/**
+ * @namespace units::radiation
+ * @brief namespace for unit types and containers representing radiation values
+ * @details The SI units for radiation are:
+ *          - source activity:	becquerel
+ *          - absorbed dose:	gray
+ *          - equivalent dose:	sievert
+ * @anchor radiationContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_RADIATION_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(radiation, becquerel, becquerels, Bq,
+                              unit<std::ratio<1>, units::frequency::hertz>)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    radiation, gray, grays, Gy,
+    compound_unit<energy::joules, inverse<mass::kilogram>>)
+UNIT_ADD_WITH_METRIC_PREFIXES(radiation, sievert, sieverts, Sv,
+                              unit<std::ratio<1>, grays>)
+UNIT_ADD(radiation, curie, curies, Ci, unit<std::ratio<37>, gigabecquerels>)
+UNIT_ADD(radiation, rutherford, rutherfords, rd,
+         unit<std::ratio<1>, megabecquerels>)
+UNIT_ADD(radiation, rad, rads, rads, unit<std::ratio<1>, centigrays>)
+
+UNIT_ADD_CATEGORY_TRAIT(radioactivity)
+#endif
+
+using namespace radiation;
+}  // namespace units

wpimath/src/main/native/include/units/solid_angle.h

diff --git a/wpimath/src/main/native/include/units/solid_angle.h b/wpimath/src/main/native/include/units/solid_angle.h
new file mode 100644
index 0000000..0e38f55
--- /dev/null
+++ b/wpimath/src/main/native/include/units/solid_angle.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/angle.h"
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::solid_angle
+ * @brief namespace for unit types and containers representing solid_angle
+ *        values
+ * @details The SI unit for solid_angle is `steradians`, and the corresponding
+ *          `base_unit` category is `solid_angle_unit`.
+ * @anchor solidAngleContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_SOLID_ANGLE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    solid_angle, steradian, steradians, sr,
+    unit<std::ratio<1>, units::category::solid_angle_unit>)
+UNIT_ADD(solid_angle, degree_squared, degrees_squared, sq_deg,
+         squared<angle::degrees>)
+UNIT_ADD(solid_angle, spat, spats, sp,
+         unit<std::ratio<4>, steradians, std::ratio<1>>)
+
+UNIT_ADD_CATEGORY_TRAIT(solid_angle)
+#endif
+
+using namespace solid_angle;
+}  // namespace units

wpimath/src/main/native/include/units/substance.h

diff --git a/wpimath/src/main/native/include/units/substance.h b/wpimath/src/main/native/include/units/substance.h
new file mode 100644
index 0000000..c774497
--- /dev/null
+++ b/wpimath/src/main/native/include/units/substance.h
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::substance
+ * @brief namespace for unit types and containers representing substance values
+ * @details The SI unit for substance is `moles`, and the corresponding
+ *          `base_unit` category is `substance_unit`.
+ * @anchor substanceContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_SUBSTANCE_UNITS)
+UNIT_ADD(substance, mole, moles, mol,
+         unit<std::ratio<1>, units::category::substance_unit>)
+
+UNIT_ADD_CATEGORY_TRAIT(substance)
+#endif
+
+using namespace substance;
+}  // namespace units

wpimath/src/main/native/include/units/temperature.h

diff --git a/wpimath/src/main/native/include/units/temperature.h b/wpimath/src/main/native/include/units/temperature.h
new file mode 100644
index 0000000..25a9b98
--- /dev/null
+++ b/wpimath/src/main/native/include/units/temperature.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+// NOTE: temperature units have special conversion overloads, since they
+// require translations and aren't a reversible transform.
+
+/**
+ * @namespace units::temperature
+ * @brief namespace for unit types and containers representing temperature
+ *        values
+ * @details The SI unit for temperature is `kelvin`, and the corresponding
+ *          `base_unit` category is `temperature_unit`.
+ * @anchor temperatureContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_TEMPERATURE_UNITS)
+UNIT_ADD(temperature, kelvin, kelvin, K,
+         unit<std::ratio<1>, units::category::temperature_unit>)
+UNIT_ADD(temperature, celsius, celsius, degC,
+         unit<std::ratio<1>, kelvin, std::ratio<0>, std::ratio<27315, 100>>)
+UNIT_ADD(temperature, fahrenheit, fahrenheit, degF,
+         unit<std::ratio<5, 9>, celsius, std::ratio<0>, std::ratio<-160, 9>>)
+UNIT_ADD(temperature, reaumur, reaumur, Re, unit<std::ratio<10, 8>, celsius>)
+UNIT_ADD(temperature, rankine, rankine, Ra, unit<std::ratio<5, 9>, kelvin>)
+
+UNIT_ADD_CATEGORY_TRAIT(temperature)
+#endif
+
+using namespace temperature;
+}  // namespace units

wpimath/src/main/native/include/units/time.h

diff --git a/wpimath/src/main/native/include/units/time.h b/wpimath/src/main/native/include/units/time.h
new file mode 100644
index 0000000..13e66c4
--- /dev/null
+++ b/wpimath/src/main/native/include/units/time.h
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::time
+ * @brief namespace for unit types and containers representing time values
+ * @details The SI unit for time is `seconds`, and the corresponding `base_unit`
+ *          category is `time_unit`.
+ * @anchor timeContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_TIME_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(time, second, seconds, s,
+                              unit<std::ratio<1>, units::category::time_unit>)
+UNIT_ADD(time, minute, minutes, min, unit<std::ratio<60>, seconds>)
+UNIT_ADD(time, hour, hours, hr, unit<std::ratio<60>, minutes>)
+UNIT_ADD(time, day, days, d, unit<std::ratio<24>, hours>)
+UNIT_ADD(time, week, weeks, wk, unit<std::ratio<7>, days>)
+UNIT_ADD(time, year, years, yr, unit<std::ratio<365>, days>)
+UNIT_ADD(time, julian_year, julian_years, a_j,
+         unit<std::ratio<31557600>, seconds>)
+UNIT_ADD(time, gregorian_year, gregorian_years, a_g,
+         unit<std::ratio<31556952>, seconds>)
+
+UNIT_ADD_CATEGORY_TRAIT(time)
+#endif
+
+using namespace time;
+}  // namespace units

wpimath/src/main/native/include/units/torque.h

diff --git a/wpimath/src/main/native/include/units/torque.h b/wpimath/src/main/native/include/units/torque.h
new file mode 100644
index 0000000..58f4ca3
--- /dev/null
+++ b/wpimath/src/main/native/include/units/torque.h
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/energy.h"
+#include "units/force.h"
+#include "units/length.h"
+
+namespace units {
+/**
+ * @namespace units::torque
+ * @brief namespace for unit types and containers representing torque values
+ * @details The SI unit for torque is `newton_meters`, and the corresponding
+ *          `base_unit` category is `torque_units`.
+ * @anchor torqueContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_TORQUE_UNITS)
+UNIT_ADD(torque, newton_meter, newton_meters, Nm,
+         unit<std::ratio<1>, units::energy::joule>)
+UNIT_ADD(torque, foot_pound, foot_pounds, ftlb,
+         compound_unit<length::foot, force::pounds>)
+UNIT_ADD(torque, foot_poundal, foot_poundals, ftpdl,
+         compound_unit<length::foot, force::poundal>)
+UNIT_ADD(torque, inch_pound, inch_pounds, inlb,
+         compound_unit<length::inch, force::pounds>)
+UNIT_ADD(torque, meter_kilogram, meter_kilograms, mkgf,
+         compound_unit<length::meter, force::kiloponds>)
+
+UNIT_ADD_CATEGORY_TRAIT(torque)
+#endif
+
+using namespace torque;
+}  // namespace units

wpimath/src/main/native/include/units/units.h

diff --git a/wpimath/src/main/native/include/units/units.h b/wpimath/src/main/native/include/units/units.h
new file mode 100644
index 0000000..8d47679
--- /dev/null
+++ b/wpimath/src/main/native/include/units/units.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+// clang-format off
+#ifdef _MSC_VER
+#pragma message("warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead.")
+#else
+#warning "Including this header drastically increases compilation times and is bad style. Include only what you use instead."
+#endif
+
+// clang-format on
+
+#include "units/acceleration.h"
+#include "units/angle.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+#include "units/area.h"
+#include "units/capacitance.h"
+#include "units/charge.h"
+#include "units/concentration.h"
+#include "units/conductance.h"
+#include "units/constants.h"
+#include "units/current.h"
+#include "units/curvature.h"
+#include "units/data.h"
+#include "units/data_transfer_rate.h"
+#include "units/density.h"
+#include "units/dimensionless.h"
+#include "units/energy.h"
+#include "units/force.h"
+#include "units/frequency.h"
+#include "units/illuminance.h"
+#include "units/impedance.h"
+#include "units/inductance.h"
+#include "units/length.h"
+#include "units/luminous_flux.h"
+#include "units/luminous_intensity.h"
+#include "units/magnetic_field_strength.h"
+#include "units/magnetic_flux.h"
+#include "units/mass.h"
+#include "units/math.h"
+#include "units/moment_of_inertia.h"
+#include "units/power.h"
+#include "units/pressure.h"
+#include "units/radiation.h"
+#include "units/solid_angle.h"
+#include "units/substance.h"
+#include "units/temperature.h"
+#include "units/time.h"
+#include "units/torque.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+#include "units/volume.h"

wpimath/src/main/native/include/units/velocity.h

diff --git a/wpimath/src/main/native/include/units/velocity.h b/wpimath/src/main/native/include/units/velocity.h
new file mode 100644
index 0000000..5a0ebcb
--- /dev/null
+++ b/wpimath/src/main/native/include/units/velocity.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/length.h"
+#include "units/time.h"
+
+namespace units {
+/**
+ * @namespace units::velocity
+ * @brief namespace for unit types and containers representing velocity values
+ * @details The SI unit for velocity is `meters_per_second`, and the
+ *          corresponding `base_unit` category is `velocity_unit`.
+ * @anchor velocityContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_VELOCITY_UNITS)
+UNIT_ADD(velocity, meters_per_second, meters_per_second, mps,
+         unit<std::ratio<1>, units::category::velocity_unit>)
+UNIT_ADD(velocity, feet_per_second, feet_per_second, fps,
+         compound_unit<length::feet, inverse<time::seconds>>)
+UNIT_ADD(velocity, miles_per_hour, miles_per_hour, mph,
+         compound_unit<length::miles, inverse<time::hour>>)
+UNIT_ADD(velocity, kilometers_per_hour, kilometers_per_hour, kph,
+         compound_unit<length::kilometers, inverse<time::hour>>)
+UNIT_ADD(velocity, knot, knots, kts,
+         compound_unit<length::nauticalMiles, inverse<time::hour>>)
+
+UNIT_ADD_CATEGORY_TRAIT(velocity)
+#endif
+
+using namespace velocity;
+}  // namespace units

wpimath/src/main/native/include/units/voltage.h

diff --git a/wpimath/src/main/native/include/units/voltage.h b/wpimath/src/main/native/include/units/voltage.h
new file mode 100644
index 0000000..917c52a
--- /dev/null
+++ b/wpimath/src/main/native/include/units/voltage.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+
+namespace units {
+/**
+ * @namespace units::voltage
+ * @brief namespace for unit types and containers representing voltage values
+ * @details The SI unit for voltage is `volts`, and the corresponding
+ *          `base_unit` category is `voltage_unit`.
+ * @anchor voltageContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_VOLTAGE_UNITS)
+UNIT_ADD_WITH_METRIC_PREFIXES(
+    voltage, volt, volts, V, unit<std::ratio<1>, units::category::voltage_unit>)
+UNIT_ADD(voltage, statvolt, statvolts, statV,
+         unit<std::ratio<1000000, 299792458>, volts>)
+UNIT_ADD(voltage, abvolt, abvolts, abV, unit<std::ratio<1, 100000000>, volts>)
+
+UNIT_ADD_CATEGORY_TRAIT(voltage)
+#endif
+
+using namespace voltage;
+}  // namespace units

wpimath/src/main/native/include/units/volume.h

diff --git a/wpimath/src/main/native/include/units/volume.h b/wpimath/src/main/native/include/units/volume.h
new file mode 100644
index 0000000..f13fdef
--- /dev/null
+++ b/wpimath/src/main/native/include/units/volume.h
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+// Copyright (c) 2016 Nic Holthaus
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include "units/base.h"
+#include "units/length.h"
+
+namespace units {
+/**
+ * @namespace units::volume
+ * @brief namespace for unit types and containers representing volume values
+ * @details The SI unit for volume is `cubic_meters`, and the corresponding
+ *          `base_unit` category is `volume_unit`.
+ * @anchor volumeContainers
+ * @sa See unit_t for more information on unit type containers.
+ */
+#if !defined(DISABLE_PREDEFINED_UNITS) || \
+    defined(ENABLE_PREDEFINED_VOLUME_UNITS)
+UNIT_ADD(volume, cubic_meter, cubic_meters, cu_m,
+         unit<std::ratio<1>, units::category::volume_unit>)
+UNIT_ADD(volume, cubic_millimeter, cubic_millimeters, cu_mm,
+         cubed<length::millimeter>)
+UNIT_ADD(volume, cubic_kilometer, cubic_kilometers, cu_km,
+         cubed<length::kilometer>)
+UNIT_ADD_WITH_METRIC_PREFIXES(volume, liter, liters, L,
+                              cubed<deci<length::meter>>)
+UNIT_ADD(volume, cubic_inch, cubic_inches, cu_in, cubed<length::inches>)
+UNIT_ADD(volume, cubic_foot, cubic_feet, cu_ft, cubed<length::feet>)
+UNIT_ADD(volume, cubic_yard, cubic_yards, cu_yd, cubed<length::yards>)
+UNIT_ADD(volume, cubic_mile, cubic_miles, cu_mi, cubed<length::miles>)
+UNIT_ADD(volume, gallon, gallons, gal, unit<std::ratio<231>, cubic_inches>)
+UNIT_ADD(volume, quart, quarts, qt, unit<std::ratio<1, 4>, gallons>)
+UNIT_ADD(volume, pint, pints, pt, unit<std::ratio<1, 2>, quarts>)
+UNIT_ADD(volume, cup, cups, c, unit<std::ratio<1, 2>, pints>)
+UNIT_ADD(volume, fluid_ounce, fluid_ounces, fl_oz, unit<std::ratio<1, 8>, cups>)
+UNIT_ADD(volume, barrel, barrels, bl, unit<std::ratio<42>, gallons>)
+UNIT_ADD(volume, bushel, bushels, bu,
+         unit<std::ratio<215042, 100>, cubic_inches>)
+UNIT_ADD(volume, cord, cords, cord, unit<std::ratio<128>, cubic_feet>)
+UNIT_ADD(volume, cubic_fathom, cubic_fathoms, cu_fm, cubed<length::fathom>)
+UNIT_ADD(volume, tablespoon, tablespoons, tbsp,
+         unit<std::ratio<1, 2>, fluid_ounces>)
+UNIT_ADD(volume, teaspoon, teaspoons, tsp, unit<std::ratio<1, 6>, fluid_ounces>)
+UNIT_ADD(volume, pinch, pinches, pinch, unit<std::ratio<1, 8>, teaspoons>)
+UNIT_ADD(volume, dash, dashes, dash, unit<std::ratio<1, 2>, pinches>)
+UNIT_ADD(volume, drop, drops, drop, unit<std::ratio<1, 360>, fluid_ounces>)
+UNIT_ADD(volume, fifth, fifths, fifth, unit<std::ratio<1, 5>, gallons>)
+UNIT_ADD(volume, dram, drams, dr, unit<std::ratio<1, 8>, fluid_ounces>)
+UNIT_ADD(volume, gill, gills, gi, unit<std::ratio<4>, fluid_ounces>)
+UNIT_ADD(volume, peck, pecks, pk, unit<std::ratio<1, 4>, bushels>)
+UNIT_ADD(volume, sack, sacks, sacks, unit<std::ratio<3>, bushels>)
+UNIT_ADD(volume, shot, shots, shots, unit<std::ratio<3, 2>, fluid_ounces>)
+UNIT_ADD(volume, strike, strikes, strikes, unit<std::ratio<2>, bushels>)
+
+UNIT_ADD_CATEGORY_TRAIT(volume)
+#endif
+
+using namespace volume;
+}  // namespace units

wpimath/src/main/native/include/unsupported/Eigen/AutoDiff

diff --git a/wpimath/src/main/native/include/unsupported/Eigen/AutoDiff b/wpimath/src/main/native/include/unsupported/Eigen/AutoDiff
new file mode 100644
index 0000000..abf5b7d
--- /dev/null
+++ b/wpimath/src/main/native/include/unsupported/Eigen/AutoDiff
@@ -0,0 +1,40 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_AUTODIFF_MODULE
+#define EIGEN_AUTODIFF_MODULE
+
+namespace Eigen {
+
+/**
+  * \defgroup AutoDiff_Module Auto Diff module
+  *
+  * This module features forward automatic differentation via a simple
+  * templated scalar type wrapper AutoDiffScalar.
+  *
+  * Warning : this should NOT be confused with numerical differentiation, which
+  * is a different method and has its own module in Eigen : \ref NumericalDiff_Module.
+  *
+  * \code
+  * #include <unsupported/Eigen/AutoDiff>
+  * \endcode
+  */
+//@{
+
+}
+
+#include "src/AutoDiff/AutoDiffScalar.h"
+// #include "src/AutoDiff/AutoDiffVector.h"
+#include "src/AutoDiff/AutoDiffJacobian.h"
+
+namespace Eigen {
+//@}
+}
+
+#endif // EIGEN_AUTODIFF_MODULE

wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions

diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions b/wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions
rename to wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions

wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h

diff --git a/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
new file mode 100644
index 0000000..33b6c39
--- /dev/null
+++ b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
@@ -0,0 +1,108 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_AUTODIFF_JACOBIAN_H
+#define EIGEN_AUTODIFF_JACOBIAN_H
+
+namespace Eigen
+{
+
+template<typename Functor> class AutoDiffJacobian : public Functor
+{
+public:
+  AutoDiffJacobian() : Functor() {}
+  AutoDiffJacobian(const Functor& f) : Functor(f) {}
+
+  // forward constructors
+#if EIGEN_HAS_VARIADIC_TEMPLATES
+  template<typename... T>
+  AutoDiffJacobian(const T& ...Values) : Functor(Values...) {}
+#else
+  template<typename T0>
+  AutoDiffJacobian(const T0& a0) : Functor(a0) {}
+  template<typename T0, typename T1>
+  AutoDiffJacobian(const T0& a0, const T1& a1) : Functor(a0, a1) {}
+  template<typename T0, typename T1, typename T2>
+  AutoDiffJacobian(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2) {}
+#endif
+
+  typedef typename Functor::InputType InputType;
+  typedef typename Functor::ValueType ValueType;
+  typedef typename ValueType::Scalar Scalar;
+
+  enum {
+    InputsAtCompileTime = InputType::RowsAtCompileTime,
+    ValuesAtCompileTime = ValueType::RowsAtCompileTime
+  };
+
+  typedef Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;
+  typedef typename JacobianType::Index Index;
+
+  typedef Matrix<Scalar, InputsAtCompileTime, 1> DerivativeType;
+  typedef AutoDiffScalar<DerivativeType> ActiveScalar;
+
+  typedef Matrix<ActiveScalar, InputsAtCompileTime, 1> ActiveInput;
+  typedef Matrix<ActiveScalar, ValuesAtCompileTime, 1> ActiveValue;
+
+#if EIGEN_HAS_VARIADIC_TEMPLATES
+  // Some compilers don't accept variadic parameters after a default parameter,
+  // i.e., we can't just write _jac=0 but we need to overload operator():
+  EIGEN_STRONG_INLINE
+  void operator() (const InputType& x, ValueType* v) const
+  {
+      this->operator()(x, v, 0);
+  }
+  template<typename... ParamsType>
+  void operator() (const InputType& x, ValueType* v, JacobianType* _jac,
+                   const ParamsType&... Params) const
+#else
+  void operator() (const InputType& x, ValueType* v, JacobianType* _jac=0) const
+#endif
+  {
+    eigen_assert(v!=0);
+
+    if (!_jac)
+    {
+#if EIGEN_HAS_VARIADIC_TEMPLATES
+      Functor::operator()(x, v, Params...);
+#else
+      Functor::operator()(x, v);
+#endif
+      return;
+    }
+
+    JacobianType& jac = *_jac;
+
+    ActiveInput ax = x.template cast<ActiveScalar>();
+    ActiveValue av(jac.rows());
+
+    if(InputsAtCompileTime==Dynamic)
+      for (Index j=0; j<jac.rows(); j++)
+        av[j].derivatives().resize(x.rows());
+
+    for (Index i=0; i<jac.cols(); i++)
+      ax[i].derivatives() = DerivativeType::Unit(x.rows(),i);
+
+#if EIGEN_HAS_VARIADIC_TEMPLATES
+    Functor::operator()(ax, &av, Params...);
+#else
+    Functor::operator()(ax, &av);
+#endif
+
+    for (Index i=0; i<jac.rows(); i++)
+    {
+      (*v)[i] = av[i].value();
+      jac.row(i) = av[i].derivatives();
+    }
+  }
+};
+
+}
+
+#endif // EIGEN_AUTODIFF_JACOBIAN_H

wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h

diff --git a/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
new file mode 100644
index 0000000..2f50e99
--- /dev/null
+++ b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
@@ -0,0 +1,694 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_AUTODIFF_SCALAR_H
+#define EIGEN_AUTODIFF_SCALAR_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename A, typename B>
+struct make_coherent_impl {
+  static void run(A&, B&) {}
+};
+
+// resize a to match b is a.size()==0, and conversely.
+template<typename A, typename B>
+void make_coherent(const A& a, const B&b)
+{
+  make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
+}
+
+template<typename _DerType, bool Enable> struct auto_diff_special_op;
+
+} // end namespace internal
+
+template<typename _DerType> class AutoDiffScalar;
+
+template<typename NewDerType>
+inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
+  return AutoDiffScalar<NewDerType>(value,der);
+}
+
+/** \class AutoDiffScalar
+  * \brief A scalar type replacement with automatic differentation capability
+  *
+  * \param _DerType the vector type used to store/represent the derivatives. The base scalar type
+  *                 as well as the number of derivatives to compute are determined from this type.
+  *                 Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf
+  *                 if the number of derivatives is not known at compile time, and/or, the number
+  *                 of derivatives is large.
+  *                 Note that _DerType can also be a reference (e.g., \c VectorXf&) to wrap a
+  *                 existing vector into an AutoDiffScalar.
+  *                 Finally, _DerType can also be any Eigen compatible expression.
+  *
+  * This class represents a scalar value while tracking its respective derivatives using Eigen's expression
+  * template mechanism.
+  *
+  * It supports the following list of global math function:
+  *  - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
+  *  - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
+  *  - internal::conj, internal::real, internal::imag, numext::abs2.
+  *
+  * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
+  * in that case, the expression template mechanism only occurs at the top Matrix level,
+  * while derivatives are computed right away.
+  *
+  */
+
+template<typename _DerType>
+class AutoDiffScalar
+  : public internal::auto_diff_special_op
+            <_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
+                                          typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value>
+{
+  public:
+    typedef internal::auto_diff_special_op
+            <_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
+                       typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value> Base;
+    typedef typename internal::remove_all<_DerType>::type DerType;
+    typedef typename internal::traits<DerType>::Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real Real;
+
+    using Base::operator+;
+    using Base::operator*;
+
+    /** Default constructor without any initialization. */
+    AutoDiffScalar() {}
+
+    /** Constructs an active scalar from its \a value,
+        and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */
+    AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
+      : m_value(value), m_derivatives(DerType::Zero(nbDer))
+    {
+      m_derivatives.coeffRef(derNumber) = Scalar(1);
+    }
+
+    /** Conversion from a scalar constant to an active scalar.
+      * The derivatives are set to zero. */
+    /*explicit*/ AutoDiffScalar(const Real& value)
+      : m_value(value)
+    {
+      if(m_derivatives.size()>0)
+        m_derivatives.setZero();
+    }
+
+    /** Constructs an active scalar from its \a value and derivatives \a der */
+    AutoDiffScalar(const Scalar& value, const DerType& der)
+      : m_value(value), m_derivatives(der)
+    {}
+
+    template<typename OtherDerType>
+    AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    , typename internal::enable_if<
+            internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
+        &&  internal::is_convertible<OtherDerType,DerType>::value , void*>::type = 0
+#endif
+    )
+      : m_value(other.value()), m_derivatives(other.derivatives())
+    {}
+
+    friend  std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a)
+    {
+      return s << a.value();
+    }
+
+    AutoDiffScalar(const AutoDiffScalar& other)
+      : m_value(other.value()), m_derivatives(other.derivatives())
+    {}
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      m_value = other.value();
+      m_derivatives = other.derivatives();
+      return *this;
+    }
+
+    inline AutoDiffScalar& operator=(const AutoDiffScalar& other)
+    {
+      m_value = other.value();
+      m_derivatives = other.derivatives();
+      return *this;
+    }
+
+    inline AutoDiffScalar& operator=(const Scalar& other)
+    {
+      m_value = other;
+      if(m_derivatives.size()>0)
+        m_derivatives.setZero();
+      return *this;
+    }
+
+//     inline operator const Scalar& () const { return m_value; }
+//     inline operator Scalar& () { return m_value; }
+
+    inline const Scalar& value() const { return m_value; }
+    inline Scalar& value() { return m_value; }
+
+    inline const DerType& derivatives() const { return m_derivatives; }
+    inline DerType& derivatives() { return m_derivatives; }
+
+    inline bool operator< (const Scalar& other) const  { return m_value <  other; }
+    inline bool operator<=(const Scalar& other) const  { return m_value <= other; }
+    inline bool operator> (const Scalar& other) const  { return m_value >  other; }
+    inline bool operator>=(const Scalar& other) const  { return m_value >= other; }
+    inline bool operator==(const Scalar& other) const  { return m_value == other; }
+    inline bool operator!=(const Scalar& other) const  { return m_value != other; }
+
+    friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a <  b.value(); }
+    friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); }
+    friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a >  b.value(); }
+    friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); }
+    friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); }
+    friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); }
+
+    template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const  { return m_value <  b.value(); }
+    template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value <= b.value(); }
+    template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const  { return m_value >  b.value(); }
+    template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value >= b.value(); }
+    template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const  { return m_value == b.value(); }
+    template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value != b.value(); }
+
+    inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
+    {
+      return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
+    }
+
+    friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
+    {
+      return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
+    }
+
+//     inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
+//     {
+//       return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
+//     }
+
+//     friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
+//     {
+//       return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
+//     }
+
+    inline AutoDiffScalar& operator+=(const Scalar& other)
+    {
+      value() += other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
+    operator+(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
+        m_value + other.value(),
+        m_derivatives + other.derivatives());
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar&
+    operator+=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      (*this) = (*this) + other;
+      return *this;
+    }
+
+    inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
+    {
+      return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
+    }
+
+    friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
+    operator-(const Scalar& a, const AutoDiffScalar& b)
+    {
+      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
+            (a - b.value(), -b.derivatives());
+    }
+
+    inline AutoDiffScalar& operator-=(const Scalar& other)
+    {
+      value() -= other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
+    operator-(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
+        m_value - other.value(),
+        m_derivatives - other.derivatives());
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar&
+    operator-=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      *this = *this - other;
+      return *this;
+    }
+
+    inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
+    operator-() const
+    {
+      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
+        -m_value,
+        -m_derivatives);
+    }
+
+    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator*(const Scalar& other) const
+    {
+      return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
+    }
+
+    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator*(const Scalar& other, const AutoDiffScalar& a)
+    {
+      return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
+    }
+
+//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator*(const Real& other) const
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         m_value * other,
+//         (m_derivatives * other));
+//     }
+//
+//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator*(const Real& other, const AutoDiffScalar& a)
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         a.value() * other,
+//         a.derivatives() * other);
+//     }
+
+    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator/(const Scalar& other) const
+    {
+      return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
+    }
+
+    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator/(const Scalar& other, const AutoDiffScalar& a)
+    {
+      return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
+    }
+
+//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator/(const Real& other) const
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         m_value / other,
+//         (m_derivatives * (Real(1)/other)));
+//     }
+//
+//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator/(const Real& other, const AutoDiffScalar& a)
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         other / a.value(),
+//         a.derivatives() * (-Real(1)/other));
+//     }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
+        CwiseBinaryOp<internal::scalar_difference_op<Scalar> EIGEN_COMMA
+          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA
+          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) >,Scalar,product) >
+    operator/(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return MakeAutoDiffScalar(
+        m_value / other.value(),
+          ((m_derivatives * other.value()) - (other.derivatives() * m_value))
+        * (Scalar(1)/(other.value()*other.value())));
+    }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
+        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
+        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
+    operator*(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return MakeAutoDiffScalar(
+        m_value * other.value(),
+        (m_derivatives * other.value()) + (other.derivatives() * m_value));
+    }
+
+    inline AutoDiffScalar& operator*=(const Scalar& other)
+    {
+      *this = *this * other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      *this = *this * other;
+      return *this;
+    }
+
+    inline AutoDiffScalar& operator/=(const Scalar& other)
+    {
+      *this = *this / other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      *this = *this / other;
+      return *this;
+    }
+
+  protected:
+    Scalar m_value;
+    DerType m_derivatives;
+
+};
+
+namespace internal {
+
+template<typename _DerType>
+struct auto_diff_special_op<_DerType, true>
+//   : auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
+//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
+{
+  typedef typename remove_all<_DerType>::type DerType;
+  typedef typename traits<DerType>::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real Real;
+
+//   typedef auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
+//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base;
+
+//   using Base::operator+;
+//   using Base::operator+=;
+//   using Base::operator-;
+//   using Base::operator-=;
+//   using Base::operator*;
+//   using Base::operator*=;
+
+  const AutoDiffScalar<_DerType>& derived() const { return *static_cast<const AutoDiffScalar<_DerType>*>(this); }
+  AutoDiffScalar<_DerType>& derived() { return *static_cast<AutoDiffScalar<_DerType>*>(this); }
+
+
+  inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
+  {
+    return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
+  }
+
+  friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<_DerType>& b)
+  {
+    return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
+  }
+
+  inline AutoDiffScalar<_DerType>& operator+=(const Real& other)
+  {
+    derived().value() += other;
+    return derived();
+  }
+
+
+  inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
+  operator*(const Real& other) const
+  {
+    return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
+      derived().value() * other,
+      derived().derivatives() * other);
+  }
+
+  friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
+  operator*(const Real& other, const AutoDiffScalar<_DerType>& a)
+  {
+    return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
+      a.value() * other,
+      a.derivatives() * other);
+  }
+
+  inline AutoDiffScalar<_DerType>& operator*=(const Scalar& other)
+  {
+    *this = *this * other;
+    return derived();
+  }
+};
+
+template<typename _DerType>
+struct auto_diff_special_op<_DerType, false>
+{
+  void operator*() const;
+  void operator-() const;
+  void operator+() const;
+};
+
+template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
+struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
+  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
+  static void run(A& a, B& b) {
+    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
+    {
+      a.resize(b.size());
+      a.setZero();
+    }
+  }
+};
+
+template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
+struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
+  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
+  static void run(A& a, B& b) {
+    if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
+    {
+      b.resize(a.size());
+      b.setZero();
+    }
+  }
+};
+
+template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
+         typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
+struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
+                             Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
+  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
+  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
+  static void run(A& a, B& b) {
+    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
+    {
+      a.resize(b.size());
+      a.setZero();
+    }
+    else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
+    {
+      b.resize(a.size());
+      b.setZero();
+    }
+  }
+};
+
+} // end namespace internal
+
+template<typename DerType, typename BinOp>
+struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
+{
+  typedef AutoDiffScalar<DerType> ReturnType;
+};
+
+template<typename DerType, typename BinOp>
+struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
+{
+  typedef AutoDiffScalar<DerType> ReturnType;
+};
+
+
+// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
+
+// template<typename DerType, typename BinOp>
+// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
+// {
+//   enum { Defined = 1 };
+//   typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
+// };
+//
+// template<typename DerType1,typename DerType2, typename BinOp>
+// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
+// {
+//   enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
+//   typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
+// };
+
+#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
+  template<typename DerType> \
+  inline const Eigen::AutoDiffScalar< \
+  EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
+  FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
+    using namespace Eigen; \
+    typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
+    EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
+    CODE; \
+  }
+
+template<typename DerType>
+inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x)  { return x; }
+template<typename DerType>
+inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x)  { return x; }
+template<typename DerType>
+inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&)    { return 0.; }
+template<typename DerType, typename T>
+inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const T& y) {
+  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
+  return (x <= y ? ADS(x) : ADS(y));
+}
+template<typename DerType, typename T>
+inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const T& y) {
+  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
+  return (x >= y ? ADS(x) : ADS(y));
+}
+template<typename DerType, typename T>
+inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const T& x, const AutoDiffScalar<DerType>& y) {
+  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
+  return (x < y ? ADS(x) : ADS(y));
+}
+template<typename DerType, typename T>
+inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const T& x, const AutoDiffScalar<DerType>& y) {
+  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
+  return (x > y ? ADS(x) : ADS(y));
+}
+template<typename DerType>
+inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
+  return (x.value() < y.value() ? x : y);
+}
+template<typename DerType>
+inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
+  return (x.value() >= y.value() ? x : y);
+}
+
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs,
+  using std::abs;
+  return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2,
+  using numext::abs2;
+  return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt,
+  using std::sqrt;
+  Scalar sqrtx = sqrt(x.value());
+  return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos,
+  using std::cos;
+  using std::sin;
+  return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin,
+  using std::sin;
+  using std::cos;
+  return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp,
+  using std::exp;
+  Scalar expx = exp(x.value());
+  return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log,
+  using std::log;
+  return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));)
+
+template<typename DerType>
+inline const Eigen::AutoDiffScalar<
+EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
+pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar &y)
+{
+  using namespace Eigen;
+  using std::pow;
+  return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1)));
+}
+
+
+template<typename DerTypeA,typename DerTypeB>
+inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
+atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
+{
+  using std::atan2;
+  typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
+  typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS;
+  PlainADS ret;
+  ret.value() = atan2(a.value(), b.value());
+  
+  Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
+  
+  // if (squared_hypot==0) the derivation is undefined and the following results in a NaN:
+  ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot;
+
+  return ret;
+}
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan,
+  using std::tan;
+  using std::cos;
+  return Eigen::MakeAutoDiffScalar(tan(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cos(x.value()))));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin,
+  using std::sqrt;
+  using std::asin;
+  return Eigen::MakeAutoDiffScalar(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-numext::abs2(x.value()))));)
+  
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos,
+  using std::sqrt;
+  using std::acos;
+  return Eigen::MakeAutoDiffScalar(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-numext::abs2(x.value()))));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tanh,
+  using std::cosh;
+  using std::tanh;
+  return Eigen::MakeAutoDiffScalar(tanh(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cosh(x.value()))));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh,
+  using std::sinh;
+  using std::cosh;
+  return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh,
+  using std::sinh;
+  using std::cosh;
+  return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));)
+
+#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY
+
+template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
+  : NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
+{
+  typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
+  typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
+                                0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
+  typedef AutoDiffScalar<DerType> NonInteger;
+  typedef AutoDiffScalar<DerType> Nested;
+  typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
+  enum{
+    RequireInitialization = 1
+  };
+};
+
+}
+
+namespace std {
+template <typename T>
+class numeric_limits<Eigen::AutoDiffScalar<T> >
+  : public numeric_limits<typename T::Scalar> {};
+
+}  // namespace std
+
+#endif // EIGEN_AUTODIFF_SCALAR_H

wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h

diff --git a/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
new file mode 100644
index 0000000..8c2d048
--- /dev/null
+++ b/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
@@ -0,0 +1,220 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_AUTODIFF_VECTOR_H
+#define EIGEN_AUTODIFF_VECTOR_H
+
+namespace Eigen {
+
+/* \class AutoDiffScalar
+  * \brief A scalar type replacement with automatic differentation capability
+  *
+  * \param DerType the vector type used to store/represent the derivatives (e.g. Vector3f)
+  *
+  * This class represents a scalar value while tracking its respective derivatives.
+  *
+  * It supports the following list of global math function:
+  *  - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
+  *  - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
+  *  - internal::conj, internal::real, internal::imag, numext::abs2.
+  *
+  * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
+  * in that case, the expression template mechanism only occurs at the top Matrix level,
+  * while derivatives are computed right away.
+  *
+  */
+template<typename ValueType, typename JacobianType>
+class AutoDiffVector
+{
+  public:
+    //typedef typename internal::traits<ValueType>::Scalar Scalar;
+    typedef typename internal::traits<ValueType>::Scalar BaseScalar;
+    typedef AutoDiffScalar<Matrix<BaseScalar,JacobianType::RowsAtCompileTime,1> > ActiveScalar;
+    typedef ActiveScalar Scalar;
+    typedef AutoDiffScalar<typename JacobianType::ColXpr> CoeffType;
+    typedef typename JacobianType::Index Index;
+
+    inline AutoDiffVector() {}
+
+    inline AutoDiffVector(const ValueType& values)
+      : m_values(values)
+    {
+      m_jacobian.setZero();
+    }
+
+
+    CoeffType operator[] (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
+    const CoeffType operator[] (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
+
+    CoeffType operator() (Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
+    const CoeffType operator() (Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
+
+    CoeffType coeffRef(Index i) { return CoeffType(m_values[i], m_jacobian.col(i)); }
+    const CoeffType coeffRef(Index i) const { return CoeffType(m_values[i], m_jacobian.col(i)); }
+
+    Index size() const { return m_values.size(); }
+
+    // FIXME here we could return an expression of the sum
+    Scalar sum() const { /*std::cerr << "sum \n\n";*/ /*std::cerr << m_jacobian.rowwise().sum() << "\n\n";*/ return Scalar(m_values.sum(), m_jacobian.rowwise().sum()); }
+
+
+    inline AutoDiffVector(const ValueType& values, const JacobianType& jac)
+      : m_values(values), m_jacobian(jac)
+    {}
+
+    template<typename OtherValueType, typename OtherJacobianType>
+    inline AutoDiffVector(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
+      : m_values(other.values()), m_jacobian(other.jacobian())
+    {}
+
+    inline AutoDiffVector(const AutoDiffVector& other)
+      : m_values(other.values()), m_jacobian(other.jacobian())
+    {}
+
+    template<typename OtherValueType, typename OtherJacobianType>
+    inline AutoDiffVector& operator=(const AutoDiffVector<OtherValueType, OtherJacobianType>& other)
+    {
+      m_values = other.values();
+      m_jacobian = other.jacobian();
+      return *this;
+    }
+
+    inline AutoDiffVector& operator=(const AutoDiffVector& other)
+    {
+      m_values = other.values();
+      m_jacobian = other.jacobian();
+      return *this;
+    }
+
+    inline const ValueType& values() const { return m_values; }
+    inline ValueType& values() { return m_values; }
+
+    inline const JacobianType& jacobian() const { return m_jacobian; }
+    inline JacobianType& jacobian() { return m_jacobian; }
+
+    template<typename OtherValueType,typename OtherJacobianType>
+    inline const AutoDiffVector<
+      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
+      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >
+    operator+(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
+    {
+      return AutoDiffVector<
+      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,ValueType,OtherValueType>::Type,
+      typename MakeCwiseBinaryOp<internal::scalar_sum_op<BaseScalar>,JacobianType,OtherJacobianType>::Type >(
+        m_values + other.values(),
+        m_jacobian + other.jacobian());
+    }
+
+    template<typename OtherValueType, typename OtherJacobianType>
+    inline AutoDiffVector&
+    operator+=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
+    {
+      m_values += other.values();
+      m_jacobian += other.jacobian();
+      return *this;
+    }
+
+    template<typename OtherValueType,typename OtherJacobianType>
+    inline const AutoDiffVector<
+      typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
+      typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >
+    operator-(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
+    {
+      return AutoDiffVector<
+        typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,ValueType,OtherValueType>::Type,
+        typename MakeCwiseBinaryOp<internal::scalar_difference_op<Scalar>,JacobianType,OtherJacobianType>::Type >(
+          m_values - other.values(),
+          m_jacobian - other.jacobian());
+    }
+
+    template<typename OtherValueType, typename OtherJacobianType>
+    inline AutoDiffVector&
+    operator-=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
+    {
+      m_values -= other.values();
+      m_jacobian -= other.jacobian();
+      return *this;
+    }
+
+    inline const AutoDiffVector<
+      typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
+      typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >
+    operator-() const
+    {
+      return AutoDiffVector<
+        typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, ValueType>::Type,
+        typename MakeCwiseUnaryOp<internal::scalar_opposite_op<Scalar>, JacobianType>::Type >(
+          -m_values,
+          -m_jacobian);
+    }
+
+    inline const AutoDiffVector<
+      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
+      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type>
+    operator*(const BaseScalar& other) const
+    {
+      return AutoDiffVector<
+        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
+        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
+          m_values * other,
+          m_jacobian * other);
+    }
+
+    friend inline const AutoDiffVector<
+      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
+      typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >
+    operator*(const Scalar& other, const AutoDiffVector& v)
+    {
+      return AutoDiffVector<
+        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, ValueType>::Type,
+        typename MakeCwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>::Type >(
+          v.values() * other,
+          v.jacobian() * other);
+    }
+
+//     template<typename OtherValueType,typename OtherJacobianType>
+//     inline const AutoDiffVector<
+//       CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
+//       CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
+//         CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
+//         CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >
+//     operator*(const AutoDiffVector<OtherValueType,OtherJacobianType>& other) const
+//     {
+//       return AutoDiffVector<
+//         CwiseBinaryOp<internal::scalar_multiple_op<Scalar>, ValueType, OtherValueType>
+//         CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
+//           CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, JacobianType>,
+//           CwiseUnaryOp<internal::scalar_multiple_op<Scalar>, OtherJacobianType> > >(
+//             m_values.cwise() * other.values(),
+//             (m_jacobian * other.values()) + (m_values * other.jacobian()));
+//     }
+
+    inline AutoDiffVector& operator*=(const Scalar& other)
+    {
+      m_values *= other;
+      m_jacobian *= other;
+      return *this;
+    }
+
+    template<typename OtherValueType,typename OtherJacobianType>
+    inline AutoDiffVector& operator*=(const AutoDiffVector<OtherValueType,OtherJacobianType>& other)
+    {
+      *this = *this * other;
+      return *this;
+    }
+
+  protected:
+    ValueType m_values;
+    JacobianType m_jacobian;
+
+};
+
+}
+
+#endif // EIGEN_AUTODIFF_VECTOR_H

wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h

diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h

wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h

diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h

wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h

diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h

wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h

diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h

wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h

diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h

wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h

diff --git a/wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
similarity index 100%
rename from wpiutil/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
rename to wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h

wpimath/src/main/native/include/wpimath/MathShared.h

diff --git a/wpimath/src/main/native/include/wpimath/MathShared.h b/wpimath/src/main/native/include/wpimath/MathShared.h
new file mode 100644
index 0000000..ea23a88
--- /dev/null
+++ b/wpimath/src/main/native/include/wpimath/MathShared.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/Twine.h>
+
+namespace wpi::math {
+
+enum class MathUsageId {
+  kKinematics_DifferentialDrive,
+  kKinematics_MecanumDrive,
+  kKinematics_SwerveDrive,
+  kTrajectory_TrapezoidProfile,
+  kFilter_Linear,
+  kOdometry_DifferentialDrive,
+  kOdometry_SwerveDrive,
+  kOdometry_MecanumDrive
+};
+
+class MathShared {
+ public:
+  virtual ~MathShared() = default;
+  virtual void ReportError(const wpi::Twine& error) = 0;
+  virtual void ReportUsage(MathUsageId id, int count) = 0;
+};
+
+class MathSharedStore {
+ public:
+  static MathShared& GetMathShared();
+
+  static void SetMathShared(std::unique_ptr<MathShared> shared);
+
+  static void ReportError(const wpi::Twine& error) {
+    GetMathShared().ReportError(error);
+  }
+
+  static void ReportUsage(MathUsageId id, int count) {
+    GetMathShared().ReportUsage(id, count);
+  }
+};
+
+}  // namespace wpi::math

wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
new file mode 100644
index 0000000..2697c6c
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+import org.ejml.simple.SimpleMatrix;
+import org.junit.jupiter.api.Test;
+
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@SuppressWarnings({"ParameterName", "LocalVariableName"})
+public 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++) {
+        assertEquals(A.get(i, j), B.get(i, j), 1e-4);
+      }
+    }
+  }
+
+  private boolean solveDAREandVerify(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q,
+                                     SimpleMatrix R) {
+    var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
+
+    // expect that x is the same as it's transpose
+    assertEquals(X.numRows(), X.numCols());
+    assertMatrixEqual(X, X.transpose());
+
+    // Verify that this is a solution to the DARE.
+    SimpleMatrix Y = A.transpose().mult(X).mult(A)
+            .minus(X)
+            .minus(A.transpose().mult(X).mult(B)
+                    .mult(((B.transpose().mult(X).mult(B)).plus(R))
+                            .invert()).mult(B.transpose()).mult(X).mult(A))
+            .plus(Q);
+    assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
+
+    return true;
+  }
+
+  @Test
+  public void testDiscreteAlgebraicRicattiEquation() {
+    int n1 = 4;
+    int m1 = 1;
+
+    // we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
+    SimpleMatrix A1 = new SimpleMatrix(n1, n1, true, new double[]{0.5, 1, 0, 0, 0, 0, 1,
+        0, 0, 0, 0, 1, 0, 0, 0, 0}).transpose();
+    SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[]{0, 0, 0, 1});
+    SimpleMatrix Q1 = new SimpleMatrix(n1, n1, true, new double[]{1, 0,
+                                                                  0, 0, 0, 0, 0, 0, 0, 0,
+                                                                  0, 0, 0, 0, 0, 0});
+    SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[]{0.25});
+    assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
+
+    SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[]{1, 1, 0, 1});
+    SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[]{0, 1});
+    SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[]{1, 0, 0, 0});
+    SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[]{0.3});
+    assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
+
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
new file mode 100644
index 0000000..10ec44c
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+public class WPIMathJNITest {
+  @Test
+  public void testLink() {
+    assertDoesNotThrow(WPIMathJNI::forceLoad);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
new file mode 100644
index 0000000..da58b29
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.Random;
+import java.util.function.DoubleFunction;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+class LinearFilterTest {
+  private static final double kFilterStep = 0.005;
+  private static final double kFilterTime = 2.0;
+  private static final double kSinglePoleIIRTimeConstant = 0.015915;
+  private static final double kHighPassTimeConstant = 0.006631;
+  private static final int kMovAvgTaps = 6;
+
+  private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
+  private static final double kHighPassExpectedOutput = 10.074717;
+  private static final double kMovAvgExpectedOutput = -10.191644;
+
+  @SuppressWarnings("ParameterName")
+  private static double getData(double t) {
+    return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
+  }
+
+  @SuppressWarnings("ParameterName")
+  private static double getPulseData(double t) {
+    if (Math.abs(t - 1.0) < 0.001) {
+      return 1.0;
+    } else {
+      return 0.0;
+    }
+  }
+
+  @Test
+  void illegalTapNumberTest() {
+    assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
+  }
+
+  /**
+   * Test if the filter reduces the noise produced by a signal generator.
+   */
+  @ParameterizedTest
+  @MethodSource("noiseFilterProvider")
+  void noiseReduceTest(final LinearFilter filter) {
+    double noiseGenError = 0.0;
+    double filterError = 0.0;
+
+    final Random gen = new Random();
+    final double kStdDev = 10.0;
+
+    for (double t = 0; t < kFilterTime; t += kFilterStep) {
+      final double theory = getData(t);
+      final double noise = gen.nextGaussian() * kStdDev;
+      filterError += Math.abs(filter.calculate(theory + noise) - theory);
+      noiseGenError += Math.abs(noise - theory);
+    }
+
+    assertTrue(noiseGenError > filterError,
+        "Filter should have reduced noise accumulation from " + noiseGenError
+            + " but failed. The filter error was " + filterError);
+  }
+
+  static Stream<LinearFilter> noiseFilterProvider() {
+    return Stream.of(
+        LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+        LinearFilter.movingAverage(kMovAvgTaps)
+    );
+  }
+
+  /**
+   * Test if the linear filters produce consistent output for a given data set.
+   */
+  @ParameterizedTest
+  @MethodSource("outputFilterProvider")
+  void outputTest(final LinearFilter filter, final DoubleFunction<Double> data,
+                  final double expectedOutput) {
+    double filterOutput = 0.0;
+    for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
+      filterOutput = filter.calculate(data.apply(t));
+    }
+
+    assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
+  }
+
+  static Stream<Arguments> outputFilterProvider() {
+    return Stream.of(
+        arguments(LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kSinglePoleIIRExpectedOutput),
+        arguments(LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kHighPassExpectedOutput),
+        arguments(LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kMovAvgExpectedOutput),
+        arguments(LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getPulseData,
+            0.0)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
new file mode 100644
index 0000000..b2b596c
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class MedianFilterTest {
+  @Test
+  void medianFilterNotFullTestEven() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+
+    assertEquals(3.5, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterNotFullTestOdd() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+    filter.calculate(7);
+
+    assertEquals(4, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterFullTestEven() {
+    MedianFilter filter = new MedianFilter(6);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(4.5, filter.calculate(99));
+  }
+
+  @Test
+  void medianFilterFullTestOdd() {
+    MedianFilter filter = new MedianFilter(5);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(5, filter.calculate(99));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
new file mode 100644
index 0000000..05c5786
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.lang.reflect.Constructor;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.stream.Stream;
+
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestFactory;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.DynamicTest.dynamicTest;
+
+@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+public abstract class UtilityClassTest<T> {
+  private final Class<T> m_clazz;
+
+  protected UtilityClassTest(Class<T> clazz) {
+    m_clazz = clazz;
+  }
+
+  @Test
+  public void singleConstructorTest() {
+    assertEquals(1, m_clazz.getDeclaredConstructors().length,
+        "More than one constructor defined");
+  }
+
+  @Test
+  public void constructorPrivateTest() {
+    Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+
+    assertFalse(constructor.canAccess(null), "Constructor is not private");
+  }
+
+  @Test
+  public void constructorReflectionTest() {
+    Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
+    constructor.setAccessible(true);
+    assertThrows(InvocationTargetException.class, constructor::newInstance);
+  }
+
+  @TestFactory
+  Stream<DynamicTest> publicMethodsStaticTestFactory() {
+    return Arrays.stream(m_clazz.getDeclaredMethods())
+        .filter(method -> Modifier.isPublic(method.getModifiers()))
+        .map(method -> dynamicTest(method.getName(),
+            () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java
new file mode 100644
index 0000000..75a29e3
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ControlAffinePlantInversionFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+            new ControlAffinePlantInversionFeedforward<N2, N1>(
+                    Nat.N2(),
+                    Nat.N1(),
+                    this::getDynamics,
+                    0.02);
+
+    assertEquals(48.0, feedforward.calculate(
+         VecBuilder.fill(2, 2),
+         VecBuilder.fill(3, 3)).get(0, 0),
+         1e-6);
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculateState() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+        new ControlAffinePlantInversionFeedforward<N2, N1>(
+            Nat.N2(),
+            Nat.N1(),
+            this::getDynamics,
+            0.02);
+
+    assertEquals(48.0, feedforward.calculate(
+            VecBuilder.fill(2, 2),
+            VecBuilder.fill(3, 3)).get(0, 0),
+            1e-6);
+  }
+
+  @SuppressWarnings("ParameterName")
+  protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
+    var result = new Matrix<>(Nat.N2(), Nat.N1());
+
+    result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x)
+            .plus(VecBuilder.fill(0, 1).times(u));
+
+    return result;
+  }
+
+  @SuppressWarnings("ParameterName")
+  protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
+    var result = new Matrix<>(Nat.N2(), Nat.N1());
+
+    result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
+
+    return result;
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java
new file mode 100644
index 0000000..8a09383
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class LinearPlantInversionFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
+
+    LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
+            new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
+
+    assertEquals(47.502599, feedforward.calculate(
+            VecBuilder.fill(2, 2),
+            VecBuilder.fill(3, 3)).get(0, 0),
+            0.002);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java
new file mode 100644
index 0000000..e047198
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class LinearQuadraticRegulatorTest {
+  public static LinearSystem<N2, N1, N1> elevatorPlant;
+  static LinearSystem<N2, N1, N1> armPlant;
+
+  static {
+    createArm();
+    createElevator();
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  public static void createArm() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 4.0;
+    var r = 0.4;
+    var J = 1d / 3d * m * r * r;
+    var G = 100.0;
+
+    armPlant = LinearSystemId.createSingleJointedArmSystem(motors, J, G);
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  public static void createElevator() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 5.0;
+    var r = 0.0181864;
+    var G = 1.0;
+    elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testLQROnElevator() {
+
+    var qElms = VecBuilder.fill(0.02, 0.4);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.00505;
+
+    var controller = new LinearQuadraticRegulator<>(
+          elevatorPlant, qElms, rElms, dt);
+
+    var k = controller.getK();
+
+    assertEquals(522.153, k.get(0, 0), 0.1);
+    assertEquals(38.2, k.get(0, 1), 0.1);
+  }
+
+  @Test
+  public 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);
+
+    assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
+    assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
+
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testLQROnArm() {
+
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 4.0;
+    var r = 0.4;
+    var G = 100.0;
+
+    var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
+
+    var qElms = VecBuilder.fill(0.01745, 0.08726);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.00505;
+
+    var controller = new LinearQuadraticRegulator<>(
+          plant, qElms, rElms, dt);
+
+    var k = controller.getK();
+
+    assertEquals(19.16, k.get(0, 0), 0.1);
+    assertEquals(3.32, k.get(0, 1), 0.1);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java
new file mode 100644
index 0000000..14a0a9d
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java
@@ -0,0 +1,169 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.controller;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.estimator.KalmanFilter;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.LinearSystemLoop;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class LinearSystemLoopTest {
+  public static final double kDt = 0.00505;
+  private static final double kPositionStddev = 0.0001;
+  private static final Random random = new Random();
+  private final LinearSystemLoop<N2, N1, N1> m_loop;
+
+  @SuppressWarnings("LocalVariableName")
+  public LinearSystemLoopTest() {
+    LinearSystem<N2, N1, N1> plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
+          0.0181864, 1.0);
+    KalmanFilter<N2, N1, N1> observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), plant,
+          VecBuilder.fill(0.05, 1.0),
+          VecBuilder.fill(0.0001), kDt);
+
+    var qElms = VecBuilder.fill(0.02, 0.4);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.00505;
+
+    var controller = new LinearQuadraticRegulator<>(
+          plant, qElms, rElms, dt);
+
+    m_loop = new LinearSystemLoop<>(plant, controller, observer, 12, dt);
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  private static void updateTwoState(LinearSystemLoop<N2, N1, N1> loop, double noise) {
+    Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
+          VecBuilder.fill(noise)
+    );
+
+    loop.correct(y);
+    loop.predict(kDt);
+  }
+
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public 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);
+    m_loop.setNextR(references);
+
+    TrapezoidProfile profile;
+    TrapezoidProfile.State state;
+    for (int i = 0; i < 1000; i++) {
+      profile = new TrapezoidProfile(
+            constraints, new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
+            new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))
+      );
+      state = profile.calculate(kDt);
+      m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
+
+      updateTwoState(m_loop, (random.nextGaussian()) * kPositionStddev);
+      var u = m_loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+    }
+
+    assertEquals(2.0, m_loop.getXHat(0), 0.05);
+    assertEquals(0.0, m_loop.getXHat(1), 0.5);
+
+  }
+
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public void testFlywheelEnabled() {
+
+    LinearSystem<N1, N1, N1> plant = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2),
+          0.00289, 1.0);
+    KalmanFilter<N1, N1, N1> observer = new KalmanFilter<>(Nat.N1(), Nat.N1(), plant,
+          VecBuilder.fill(1.0),
+          VecBuilder.fill(kPositionStddev), kDt);
+
+    var qElms = VecBuilder.fill(9.0);
+    var rElms = VecBuilder.fill(12.0);
+
+    var controller = new LinearQuadraticRegulator<>(
+          plant, qElms, rElms, kDt);
+
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
+
+    var loop = new LinearSystemLoop<>(plant, controller, feedforward, observer, 12);
+
+    loop.reset(VecBuilder.fill(0.0));
+    var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
+
+    loop.setNextR(references);
+
+    List<Double> timeData = new ArrayList<>();
+    List<Double> xHat = new ArrayList<>();
+    List<Double> volt = new ArrayList<>();
+    List<Double> reference = new ArrayList<>();
+    List<Double> error = new ArrayList<>();
+
+    var time = 0.0;
+    while (time < 10) {
+
+      if (time > 10) {
+        break;
+      }
+
+      loop.setNextR(references);
+
+      Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
+            VecBuilder.fill(random.nextGaussian() * kPositionStddev)
+      );
+
+      loop.correct(y);
+      loop.predict(kDt);
+      var u = loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+
+      xHat.add(loop.getXHat(0) / 2d / Math.PI * 60);
+      volt.add(u);
+      timeData.add(time);
+      reference.add(references.get(0, 0) / 2d / Math.PI * 60);
+      error.add(loop.getError(0) / 2d / Math.PI * 60);
+      time += kDt;
+    }
+
+    //    XYChart bigChart = new XYChartBuilder().build();
+    //    bigChart.addSeries("Xhat, RPM", timeData, xHat);
+    //    bigChart.addSeries("Reference, RPM", timeData, reference);
+    //    bigChart.addSeries("Error, RPM", timeData, error);
+
+    //    XYChart smolChart = new XYChartBuilder().build();
+    //    smolChart.addSeries("Volts, V", timeData, volt);
+
+    //  try {
+    //      new SwingWrapper<>(List.of(bigChart, smolChart)).displayChartMatrix();
+    //      Thread.sleep(10000000);
+    //    } catch (InterruptedException e) { e.printStackTrace(); }
+
+    assertEquals(0.0, loop.getError(0), 0.1);
+  }
+
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
new file mode 100644
index 0000000..6c7cc92
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
@@ -0,0 +1,219 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.NumericalJacobian;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N5;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+@SuppressWarnings("ParameterName")
+public class ExtendedKalmanFilterTest {
+  public 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
+    final var rb = 0.8382 / 2.0; // Wheelbase radius (track width)
+    final var r = 0.0746125; // Wheel radius
+    final var m = 63.503; // Robot mass
+    final var J = 5.6; // Robot moment of inertia
+
+    final var C1 =
+          -Math.pow(gr, 2) * motors.m_KtNMPerAmp / (
+                motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
+    final var C2 = gr * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
+    final var k1 = 1.0 / m + rb * rb / J;
+    final var k2 = 1.0 / m - rb * rb / J;
+
+    final var vl = x.get(3, 0);
+    final var vr = x.get(4, 0);
+    final var Vl = u.get(0, 0);
+    final var Vr = u.get(1, 0);
+
+    final Matrix<N5, N1> result = new Matrix<>(Nat.N5(), Nat.N1());
+    final var v = 0.5 * (vl + vr);
+    result.set(0, 0, v * Math.cos(x.get(2, 0)));
+    result.set(1, 0, v * Math.sin(x.get(2, 0)));
+    result.set(2, 0, (vr - vl) / (2.0 * rb));
+    result.set(3, 0, k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)));
+    result.set(4, 0, k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
+    return result;
+  }
+
+  public 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) {
+    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() {
+    double dtSeconds = 0.00505;
+
+    assertDoesNotThrow(() -> {
+      ExtendedKalmanFilter<N5, N2, N3> observer =
+            new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
+                  ExtendedKalmanFilterTest::getDynamics,
+                  ExtendedKalmanFilterTest::getLocalMeasurementModel,
+                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                  VecBuilder.fill(0.0001, 0.01, 0.01), dtSeconds);
+
+      Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
+      observer.predict(u, dtSeconds);
+
+      var localY = getLocalMeasurementModel(observer.getXhat(), u);
+      observer.correct(u, localY);
+
+      var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+      var R = StateSpaceUtil.makeCostMatrix(
+            VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+      observer.correct(Nat.N5(),
+            u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
+    });
+
+  }
+
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
+      "PMD.ExcessiveMethodLength"})
+  @Test
+  public void testConvergence() {
+    double dtSeconds = 0.00505;
+    double rbMeters = 0.8382 / 2.0; // Robot radius
+
+    ExtendedKalmanFilter<N5, N2, N3> observer =
+          new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
+                ExtendedKalmanFilterTest::getDynamics,
+                ExtendedKalmanFilterTest::getLocalMeasurementModel,
+                VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                VecBuilder.fill(0.001, 0.01, 0.01), dtSeconds);
+
+    List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
+          new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+          waypoints,
+          new TrajectoryConfig(8.8, 0.1)
+    );
+
+    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
+
+    Matrix<N5, N1> nextR = new Matrix<>(Nat.N5(), Nat.N1());
+    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
+
+    List<Double> trajXs = new ArrayList<>();
+    List<Double> trajYs = new ArrayList<>();
+
+    List<Double> observerXs = new ArrayList<>();
+    List<Double> observerYs = new ArrayList<>();
+
+    var B = NumericalJacobian.numericalJacobianU(Nat.N5(), Nat.N2(),
+          ExtendedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N5(), Nat.N1()), u);
+
+    observer.setXhat(VecBuilder.fill(
+          trajectory.getInitialPose().getTranslation().getX(),
+          trajectory.getInitialPose().getTranslation().getY(),
+          trajectory.getInitialPose().getRotation().getRadians(),
+          0.0, 0.0));
+
+    var groundTruthX = observer.getXhat();
+
+    double totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / dtSeconds); i++) {
+      var ref = trajectory.sample(dtSeconds * i);
+      double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
+      double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
+
+      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
+      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
+      nextR.set(2, 0, ref.poseMeters.getRotation().getRadians());
+      nextR.set(3, 0, vl);
+      nextR.set(4, 0, vr);
+
+      var localY =
+            getLocalMeasurementModel(groundTruthX, u);
+      var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
+      observer.correct(u,
+            localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
+
+      Matrix<N5, N1> rdot = nextR.minus(r).div(dtSeconds);
+      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+
+      observer.predict(u, dtSeconds);
+
+      groundTruthX = RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics,
+            groundTruthX, u, dtSeconds);
+
+      r = nextR;
+
+      trajXs.add(ref.poseMeters.getTranslation().getX());
+      trajYs.add(ref.poseMeters.getTranslation().getY());
+
+      observerXs.add(observer.getXhat().get(0, 0));
+      observerYs.add(observer.getXhat().get(1, 0));
+    }
+
+    var localY = getLocalMeasurementModel(observer.getXhat(), u);
+    observer.correct(u, localY);
+
+    var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+    var R = StateSpaceUtil.makeCostMatrix(
+          VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+    observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
+
+    //    var chartBuilder = new XYChartBuilder();
+    //    chartBuilder.title = "The Magic of Sensor Fusion, now with a "
+    //          + observer.getClass().getSimpleName();
+    //    var xyPosChart = chartBuilder.build();
+    //
+    //    xyPosChart.setXAxisTitle("X pos, meters");
+    //    xyPosChart.setYAxisTitle("Y pos, meters");
+    //    xyPosChart.addSeries("Trajectory", trajXs, trajYs);
+    //    xyPosChart.addSeries("xHat", observerXs, observerYs);
+    //    new SwingWrapper<>(xyPosChart).displayChart();
+    //    try {
+    //      Thread.sleep(1000000000);
+    //    } catch (InterruptedException ex) {
+    //      ex.printStackTrace();
+    //    }
+
+    var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
+    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 1.0);
+    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 1.0);
+    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
+    assertEquals(0.0, observer.getXhat(3), 1.0);
+    assertEquals(0.0, observer.getXhat(4), 1.0);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java
new file mode 100644
index 0000000..2a434fb
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java
@@ -0,0 +1,258 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N6;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class KalmanFilterTest {
+  private static LinearSystem<N2, N1, N1> elevatorPlant;
+
+  private static final double kDt = 0.00505;
+
+  static {
+    createElevator();
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  public static void createElevator() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 5.0;
+    var r = 0.0181864;
+    var G = 1.0;
+    elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
+  }
+
+  // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]^T,
+  // Y is [x, y, theta]^T and u is [ax, ay, alpha}^T
+  LinearSystem<N6, N3, N3> m_swerveObserverSystem = new LinearSystem<>(
+      Matrix.mat(Nat.N6(), Nat.N6()).fill( // A
+              0, 0, 0, 1, 0, 0,
+              0, 0, 0, 0, 1, 0,
+              0, 0, 0, 0, 0, 1,
+              0, 0, 0, 0, 0, 0,
+              0, 0, 0, 0, 0, 0,
+              0, 0, 0, 0, 0, 0),
+        Matrix.mat(Nat.N6(), Nat.N3()).fill( // B
+              0, 0, 0,
+              0, 0, 0,
+              0, 0, 0,
+              1, 0, 0,
+              0, 1, 0,
+              0, 0, 1
+        ),
+        Matrix.mat(Nat.N3(), Nat.N6()).fill( // C
+              1, 0, 0, 0, 0, 0,
+              0, 1, 0, 0, 0, 0,
+              0, 0, 1, 0, 0, 0
+        ),
+        new Matrix<>(Nat.N3(), Nat.N3())); // D
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testElevatorKalmanFilter() {
+
+    var Q = VecBuilder.fill(0.05, 1.0);
+    var R = VecBuilder.fill(0.0001);
+
+    assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt));
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public void testSwerveKFStationary() {
+
+    var random = new Random();
+
+    var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
+          m_swerveObserverSystem,
+          VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+          // weights
+          VecBuilder.fill(2, 2, 2), // measurement weights
+          0.020
+    );
+
+    List<Double> xhatsX = new ArrayList<>();
+    List<Double> measurementsX = new ArrayList<>();
+    List<Double> xhatsY = new ArrayList<>();
+    List<Double> measurementsY = new ArrayList<>();
+
+    Matrix<N3, N1> measurement;
+    for (int i = 0; i < 100; i++) {
+      // the robot is at [0, 0, 0] so we just park here
+      measurement = VecBuilder.fill(
+            random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
+      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+
+      // we continue to not accelerate
+      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+
+      measurementsX.add(measurement.get(0, 0));
+      measurementsY.add(measurement.get(1, 0));
+      xhatsX.add(filter.getXhat(0));
+      xhatsY.add(filter.getXhat(1));
+    }
+
+    //var chart = new XYChartBuilder().build();
+    //chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
+    //chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
+    //try {
+    //  new SwingWrapper<>(chart).displayChart();
+    //  Thread.sleep(10000000);
+    //} catch (Exception ign) {
+    //}
+    assertEquals(0.0, filter.getXhat(0), 0.3);
+    assertEquals(0.0, filter.getXhat(0), 0.3);
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public void testSwerveKFMovingWithoutAccelerating() {
+
+    var random = new Random();
+
+    var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
+          m_swerveObserverSystem,
+          VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+          // weights
+          VecBuilder.fill(4, 4, 4), // measurement weights
+          0.020
+    );
+
+    List<Double> xhatsX = new ArrayList<>();
+    List<Double> measurementsX = new ArrayList<>();
+    List<Double> xhatsY = new ArrayList<>();
+    List<Double> measurementsY = new ArrayList<>();
+
+    // we set the velocity of the robot so that it's moving forward slowly
+    filter.setXhat(0, 0.5);
+    filter.setXhat(1, 0.5);
+
+    for (int i = 0; i < 300; i++) {
+      // the robot is at [0, 0, 0] so we just park here
+      var measurement = VecBuilder.fill(
+            random.nextGaussian() / 10d,
+            random.nextGaussian() / 10d,
+            random.nextGaussian() / 4d // std dev of [1, 1, 1]
+      );
+
+      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+
+      // we continue to not accelerate
+      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+
+      measurementsX.add(measurement.get(0, 0));
+      measurementsY.add(measurement.get(1, 0));
+      xhatsX.add(filter.getXhat(0));
+      xhatsY.add(filter.getXhat(1));
+    }
+
+    //var chart = new XYChartBuilder().build();
+    //chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
+    //chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
+    //try {
+    //  new SwingWrapper<>(chart).displayChart();
+    //  Thread.sleep(10000000);
+    //} catch (Exception ign) {}
+
+    assertEquals(0.0, filter.getXhat(0), 0.2);
+    assertEquals(0.0, filter.getXhat(1), 0.2);
+  }
+
+  @Test
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public void testSwerveKFMovingOverTrajectory() {
+
+    var random = new Random();
+
+    var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
+          m_swerveObserverSystem,
+          VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+          // weights
+          VecBuilder.fill(4, 4, 4), // measurement weights
+          0.020
+    );
+
+    List<Double> xhatsX = new ArrayList<>();
+    List<Double> measurementsX = new ArrayList<>();
+    List<Double> xhatsY = new ArrayList<>();
+    List<Double> measurementsY = new ArrayList<>();
+
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+          List.of(
+                new Pose2d(0, 0, new Rotation2d()),
+                new Pose2d(5, 5, new Rotation2d())
+          ), new TrajectoryConfig(2, 2)
+    );
+    var time = 0.0;
+    var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
+
+    while (time <= trajectory.getTotalTimeSeconds()) {
+      var sample = trajectory.sample(time);
+      var measurement = VecBuilder.fill(
+            sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d,
+            sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d,
+            sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d
+      );
+
+      var velocity = VecBuilder.fill(
+            sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(),
+            sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(),
+            sample.curvatureRadPerMeter * sample.velocityMetersPerSecond
+      );
+      var u = (velocity.minus(lastVelocity)).div(0.020);
+      lastVelocity = velocity;
+
+      filter.correct(u, measurement);
+      filter.predict(u, 0.020);
+
+      measurementsX.add(measurement.get(0, 0));
+      measurementsY.add(measurement.get(1, 0));
+      xhatsX.add(filter.getXhat(0));
+      xhatsY.add(filter.getXhat(1));
+
+      time += 0.020;
+    }
+
+    //var chart = new XYChartBuilder().build();
+    //chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
+    //chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
+    //try {
+    //        new SwingWrapper<>(chart).displayChart();
+    //        Thread.sleep(10000000);
+    //} catch (Exception ign) {
+    //}
+
+    assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
+          .getTranslation().getX(), filter.getXhat(0), 0.2);
+    assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
+          .getTranslation().getY(), filter.getXhat(1), 0.2);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java
new file mode 100644
index 0000000..529a3c5
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class MerweScaledSigmaPointsTest {
+  @Test
+  public void testZeroMeanPoints() {
+    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
+    var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(0, 0),
+          Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
+
+    assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill(
+            0.0, 0.00173205, 0.0, -0.00173205, 0.0,
+            0.0, 0.0, 0.00173205, 0.0, -0.00173205
+    ), 1E-6));
+  }
+
+  @Test
+  public void testNonzeroMeanPoints() {
+    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
+    var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(1, 2),
+          Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
+
+    assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill(
+            1.0, 1.00173205, 1.0, 0.99826795, 1.0,
+            2.0, 2.0, 2.00547723, 2.0, 1.99452277
+    ), 1E-6));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
new file mode 100644
index 0000000..c4340ae
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
@@ -0,0 +1,396 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.estimator;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.math.Discretization;
+import edu.wpi.first.wpilibj.math.StateSpaceUtil;
+import edu.wpi.first.wpilibj.system.NumericalJacobian;
+import edu.wpi.first.wpilibj.system.RungeKutta;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpiutil.math.numbers.N4;
+import edu.wpi.first.wpiutil.math.numbers.N6;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+
+public class UnscentedKalmanFilterTest {
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+    var motors = DCMotor.getCIM(2);
+
+    var gHigh = 7.08;
+    var rb = 0.8382 / 2.0;
+    var r = 0.0746125;
+    var m = 63.503;
+    var J = 5.6;
+
+    var C1 = -Math.pow(gHigh, 2) * motors.m_KtNMPerAmp
+          / (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
+    var C2 = gHigh * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
+
+    var c = x.get(2, 0);
+    var s = x.get(3, 0);
+    var vl = x.get(4, 0);
+    var vr = x.get(5, 0);
+
+    var Vl = u.get(0, 0);
+    var Vr = u.get(1, 0);
+
+    var k1 = 1.0 / m + rb * rb / J;
+    var k2 = 1.0 / m - rb * rb / J;
+
+    var xvel = (vl + vr) / 2;
+    var w = (vr - vl) / (2.0 * rb);
+
+    return VecBuilder.fill(
+          xvel * c,
+          xvel * s,
+          -s * w,
+          c * w,
+          k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
+          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) {
+    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) {
+    return x.copy();
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testInit() {
+    assertDoesNotThrow(() -> {
+      UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
+            Nat.N6(), Nat.N4(),
+            UnscentedKalmanFilterTest::getDynamics,
+            UnscentedKalmanFilterTest::getLocalMeasurementModel,
+            VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
+            VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
+            0.00505);
+
+      var u = VecBuilder.fill(12.0, 12.0);
+      observer.predict(u, 0.00505);
+
+      var localY = getLocalMeasurementModel(observer.getXhat(), u);
+      observer.correct(u, localY);
+    });
+  }
+
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
+        "PMD.ExcessiveMethodLength"})
+  @Test
+  public void testConvergence() {
+    double dtSeconds = 0.00505;
+    double rbMeters = 0.8382 / 2.0; // Robot radius
+
+    List<Double> trajXs = new ArrayList<>();
+    List<Double> trajYs = new ArrayList<>();
+
+    List<Double> observerXs = new ArrayList<>();
+    List<Double> observerYs = new ArrayList<>();
+    List<Double> observerC = new ArrayList<>();
+    List<Double> observerS = new ArrayList<>();
+    List<Double> observervl = new ArrayList<>();
+    List<Double> observervr = new ArrayList<>();
+
+    List<Double> inputVl = new ArrayList<>();
+    List<Double> inputVr = new ArrayList<>();
+
+    List<Double> timeData = new ArrayList<>();
+    List<Matrix<?, ?>> rdots = new ArrayList<>();
+
+    UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
+          Nat.N6(), Nat.N4(),
+          UnscentedKalmanFilterTest::getDynamics,
+          UnscentedKalmanFilterTest::getLocalMeasurementModel,
+          VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
+          VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
+          dtSeconds);
+
+    List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
+          new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+          waypoints,
+          new TrajectoryConfig(8.8, 0.1)
+    );
+
+    Matrix<N6, N1> nextR;
+    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
+
+    var B = NumericalJacobian.numericalJacobianU(Nat.N6(), Nat.N2(),
+          UnscentedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N6(), Nat.N1()), u);
+
+    observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
+
+    var ref = trajectory.sample(0.0);
+
+    Matrix<N6, N1> r = VecBuilder.fill(
+          ref.poseMeters.getTranslation().getX(),
+          ref.poseMeters.getTranslation().getY(),
+          ref.poseMeters.getRotation().getCos(),
+          ref.poseMeters.getRotation().getSin(),
+          ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
+          ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters))
+    );
+    nextR = r.copy();
+
+    var trueXhat = observer.getXhat();
+
+    double totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / dtSeconds); i++) {
+
+      ref = trajectory.sample(dtSeconds * i);
+      double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
+      double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
+
+      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
+      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
+      nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
+      nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
+      nextR.set(4, 0, vl);
+      nextR.set(5, 0, vr);
+
+      Matrix<N4, N1> localY =
+            getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
+
+      observer.correct(u,
+            localY.plus(StateSpaceUtil.makeWhiteNoiseVector(
+                  noiseStdDev)));
+
+      var rdot = nextR.minus(r).div(dtSeconds);
+      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+
+      rdots.add(rdot);
+
+      trajXs.add(ref.poseMeters.getTranslation().getX());
+      trajYs.add(ref.poseMeters.getTranslation().getY());
+
+      observerXs.add(observer.getXhat().get(0, 0));
+      observerYs.add(observer.getXhat().get(1, 0));
+
+      observerC.add(observer.getXhat(2));
+      observerS.add(observer.getXhat(3));
+
+      observervl.add(observer.getXhat(4));
+      observervr.add(observer.getXhat(5));
+
+      inputVl.add(u.get(0, 0));
+      inputVr.add(u.get(1, 0));
+
+      timeData.add(i * dtSeconds);
+
+      r = nextR;
+      observer.predict(u, dtSeconds);
+      trueXhat = RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics,
+            trueXhat, u, dtSeconds);
+    }
+
+    var localY = getLocalMeasurementModel(trueXhat, u);
+    observer.correct(u, localY);
+
+    var globalY = getGlobalMeasurementModel(trueXhat, u);
+    var R = StateSpaceUtil.makeCostMatrix(
+          VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
+    observer.correct(Nat.N6(), u, globalY,
+          UnscentedKalmanFilterTest::getGlobalMeasurementModel, R);
+
+    final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+    //     var chartBuilder = new XYChartBuilder();
+    //     chartBuilder.title = "The Magic of Sensor Fusion, now with a "
+    //           + observer.getClass().getSimpleName();
+    //     var xyPosChart = chartBuilder.build();
+
+    //     xyPosChart.setXAxisTitle("X pos, meters");
+    //     xyPosChart.setYAxisTitle("Y pos, meters");
+    //     xyPosChart.addSeries("Trajectory", trajXs, trajYs);
+    //     xyPosChart.addSeries("xHat", observerXs, observerYs);
+
+    //     var stateChart = new XYChartBuilder()
+    //           .title("States (x-hat)").build();
+    //     stateChart.addSeries("Cos", timeData, observerC);
+    //     stateChart.addSeries("Sin", timeData, observerS);
+    //     stateChart.addSeries("vl, m/s", timeData, observervl);
+    //     stateChart.addSeries("vr, m/s", timeData, observervr);
+
+    //     var inputChart = new XYChartBuilder().title("Inputs").build();
+    //     inputChart.addSeries("Left voltage", timeData, inputVl);
+    //     inputChart.addSeries("Right voltage", timeData, inputVr);
+
+    //     var rdotChart = new XYChartBuilder().title("Rdot").build();
+    //     rdotChart.addSeries("xdot, or vx", timeData, rdots.stream().map(it -> it.get(0, 0))
+    //           .collect(Collectors.toList()));
+    //     rdotChart.addSeries("ydot, or vy", timeData, rdots.stream().map(it -> it.get(1, 0))
+    //           .collect(Collectors.toList()));
+    //     rdotChart.addSeries("cos dot", timeData, rdots.stream().map(it -> it.get(2, 0))
+    //           .collect(Collectors.toList()));
+    //     rdotChart.addSeries("sin dot", timeData, rdots.stream().map(it -> it.get(3, 0))
+    //           .collect(Collectors.toList()));
+    //     rdotChart.addSeries("vl dot, or al", timeData, rdots.stream().map(it -> it.get(4, 0))
+    //           .collect(Collectors.toList()));
+    //     rdotChart.addSeries("vr dot, or ar", timeData, rdots.stream().map(it -> it.get(5, 0))
+    //           .collect(Collectors.toList()));
+
+    //     List<XYChart> charts = new ArrayList<>();
+    //     charts.add(xyPosChart);
+    //     charts.add(stateChart);
+    //     charts.add(inputChart);
+    //     charts.add(rdotChart);
+    //     new SwingWrapper<>(charts).displayChartMatrix();
+    //     try {
+    //       Thread.sleep(1000000000);
+    //     } catch (InterruptedException ex) {
+    //       ex.printStackTrace();
+    //     }
+
+    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
+    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
+    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
+    assertEquals(0.0, observer.getXhat(3), 1.0);
+    assertEquals(0.0, observer.getXhat(4), 1.0);
+  }
+
+  @Test
+  @SuppressWarnings({"LocalVariableName", "ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  public void testLinearUKF() {
+    var dt = 0.020;
+    var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
+    var observer = new UnscentedKalmanFilter<>(Nat.N1(), Nat.N1(),
+        (x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
+          plant::calculateY,
+          VecBuilder.fill(0.05),
+          VecBuilder.fill(1.0),
+          dt);
+
+    var time = new ArrayList<Double>();
+    var refData = new ArrayList<Double>();
+    var xhat = new ArrayList<Double>();
+    var udata = new ArrayList<Double>();
+    var xdotData = new ArrayList<Double>();
+
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    Matrix<N1, N1> ref = VecBuilder.fill(100);
+    Matrix<N1, N1> u = VecBuilder.fill(0);
+
+    Matrix<N1, N1> xdot;
+    for (int i = 0; i < (2.0 / dt); i++) {
+      observer.predict(u, dt);
+
+      u = discB.solve(ref.minus(discA.times(ref)));
+
+      xdot = plant.getA().times(observer.getXhat()).plus(plant.getB().times(u));
+
+      time.add(i * dt);
+      refData.add(ref.get(0, 0));
+      xhat.add(observer.getXhat(0));
+      udata.add(u.get(0, 0));
+      xdotData.add(xdot.get(0, 0));
+    }
+
+    //    var chartBuilder = new XYChartBuilder();
+    //    chartBuilder.title = "The Magic of Sensor Fusion";
+    //    var chart = chartBuilder.build();
+
+    //    chart.addSeries("Ref", time, refData);
+    //    chart.addSeries("xHat", time, xhat);
+    //    chart.addSeries("input", time, udata);
+    ////    chart.addSeries("xdot", time, xdotData);
+
+    //    new SwingWrapper<>(chart).displayChart();
+    //    try {
+    //      Thread.sleep(1000000000);
+    //    } catch (InterruptedException e) {
+    //    }
+
+    assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
+  }
+
+  @Test
+  public void testUnscentedTransform() {
+    // From FilterPy
+    var ret = UnscentedKalmanFilter.unscentedTransform(Nat.N4(), Nat.N4(),
+          Matrix.mat(Nat.N4(), Nat.N9()).fill(
+              -0.9, -0.822540333075852, -0.8922540333075852, -0.9,
+                  -0.9, -0.9774596669241481, -0.9077459666924148, -0.9, -0.9,
+              1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519, 1.0, 1.0,
+              -0.9, -0.9, -0.9, -0.822540333075852, -0.8922540333075852, -0.9,
+                  -0.9, -0.9774596669241481, -0.9077459666924148,
+              1.0, 1.0, 1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519
+          ),
+          VecBuilder.fill(
+              -132.33333333,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667
+          ),
+          VecBuilder.fill(
+              -129.34333333,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667,
+              16.66666667
+          )
+    );
+
+    assertTrue(
+          VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(
+          ret.getFirst(), 1E-5
+    ));
+
+    assertTrue(
+            Matrix.mat(Nat.N4(), Nat.N4()).fill(
+                  2.02000002e-01, 2.00000500e-02, -2.69044710e-29,
+                  -4.59511477e-29,
+                  2.00000500e-02, 2.00001000e-01, -2.98781068e-29,
+                  -5.12759588e-29,
+                  -2.73372625e-29, -3.09882635e-29, 2.02000002e-01,
+                  2.00000500e-02,
+                  -4.67065917e-29, -5.10705197e-29, 2.00000500e-02,
+                  2.00001000e-01
+            ).isEqual(
+            ret.getSecond(), 1E-5
+    ));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
new file mode 100644
index 0000000..14bad1f
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Pose2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testTransformBy() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
+        Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRelativeTo() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0),
+            kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
+    assertNotEquals(one, two);
+  }
+
+  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));
+
+    final var transform = last.minus(initial);
+
+    assertAll(
+        () -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transform.getY(), 0.0, kEpsilon),
+        () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
new file mode 100644
index 0000000..8a08944
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Rotation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testRadiansToDegrees() {
+    var one = new Rotation2d(Math.PI / 3);
+    var two = new Rotation2d(Math.PI / 4);
+
+    assertAll(
+        () -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
+        () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    var one = Rotation2d.fromDegrees(45.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertAll(
+        () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
+        () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByFromZero() {
+    var zero = new Rotation2d();
+    var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
+        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByNonZero() {
+    var rot = Rotation2d.fromDegrees(90.0);
+    rot = rot.plus(Rotation2d.fromDegrees(30.0));
+
+    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+  }
+
+  @Test
+  void testMinus() {
+    var one = Rotation2d.fromDegrees(70.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
+  }
+
+  @Test
+  void testEquality() {
+    var one = Rotation2d.fromDegrees(43.0);
+    var two = Rotation2d.fromDegrees(43.0);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = Rotation2d.fromDegrees(43.0);
+    var two = Rotation2d.fromDegrees(43.5);
+    assertNotEquals(one, two);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java
new file mode 100644
index 0000000..c375d22
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Transform2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testInverse() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
+        Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+    var untransformed = transformed.plus(transformation.inverse());
+
+    assertAll(
+        () -> assertEquals(initial.getX(), untransformed.getX(),
+                           kEpsilon),
+        () -> assertEquals(initial.getY(), untransformed.getY(),
+                           kEpsilon),
+        () -> assertEquals(initial.getRotation().getDegrees(),
+                           untransformed.getRotation().getDegrees(), kEpsilon)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
new file mode 100644
index 0000000..d6844a5
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Translation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testSum() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
+        () -> assertEquals(sum.getY(), 8.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
+        () -> assertEquals(difference.getY(), -2.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateBy() {
+    var another = new Translation2d(3.0, 0.0);
+    var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
+        () -> assertEquals(rotated.getY(), 3.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation2d(3.0, 5.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
+        () -> assertEquals(mult.getY(), 15.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation2d(3.0, 5.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(div.getX(), 1.5, kEpsilon),
+        () -> assertEquals(div.getY(), 2.5, kEpsilon)
+    );
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation2d(-4.5, 7);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
+        () -> assertEquals(inverted.getY(), -7, kEpsilon)
+    );
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.5);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.7);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPolarConstructor() {
+    var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
+    var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
+    assertAll(
+        () -> assertEquals(one.getX(), 1.0, kEpsilon),
+        () -> assertEquals(one.getY(), 1.0, kEpsilon),
+        () -> assertEquals(two.getX(), 1.0, kEpsilon),
+        () -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
new file mode 100644
index 0000000..18ea6d9
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+class Twist2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testStraightLineTwist() {
+    var straight = new Twist2d(5.0, 0.0, 0.0);
+    var straightPose = new Pose2d().exp(straight);
+
+    assertAll(
+        () -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
+        () -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
+        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testQuarterCirleTwist() {
+    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
+    var quarterCirclePose = new Pose2d().exp(quarterCircle);
+
+    assertAll(
+        () -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist2d(2.0, 2.0, 0.0);
+    var diagonalPose = new Pose2d().exp(diagonal);
+
+    assertAll(
+        () -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1, 3);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1.2, 3);
+    assertNotEquals(one, two);
+  }
+
+  void testPose2dLog() {
+    final var start = new Pose2d();
+    final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
+
+    final var twist = start.log(end);
+
+    assertAll(
+        () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
+        () -> assertEquals(twist.dy, 0.0, kEpsilon),
+        () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
new file mode 100644
index 0000000..b06abe6
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ChassisSpeedsTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testFieldRelativeConstruction() {
+    final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+        1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)
+    );
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
new file mode 100644
index 0000000..9d2ad4e
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class DifferentialDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+  private final DifferentialDriveKinematics m_kinematics
+      = new DifferentialDriveKinematics(0.381 * 2);
+
+  @Test
+  void testInverseKinematicsForZeros() {
+    var chassisSpeeds = new ChassisSpeeds();
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testForwardKinematicsForZeros() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds();
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testInverseKinematicsForStraightLine() {
+    var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testForwardKinematicsForStraightLine() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testInverseKinematicsForRotateInPlace() {
+    var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testForwardKinematicsForRotateInPlace() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
new file mode 100644
index 0000000..e6022de
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class DifferentialDriveOdometryTest {
+  private static final double kEpsilon = 1E-9;
+  private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
+      new Rotation2d());
+
+  @Test
+  void testOdometryWithEncoderDistances() {
+    m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+    var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getY(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
+    );
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
new file mode 100644
index 0000000..93c8d6a
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
@@ -0,0 +1,262 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  @Test
+  void testStraightLineInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
+      */
+
+    assertAll(
+        () -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testStraightLineForwardKinematicsKinematics() {
+
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
+      */
+
+    assertAll(
+        () -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testStrafeInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
+      */
+
+    assertAll(
+        () -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testStrafeForwardKinematicsKinematics() {
+
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
+      */
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
+      */
+
+    assertAll(
+        () -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should be [[0][0][2pi]]
+      */
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testMixedTranslationRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
+      */
+
+    assertAll(
+        () -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testMixedTranslationRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be [[2][3][1]]
+      */
+
+    assertAll(
+        () -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
+      */
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
+    velocities should be [[12][-12][1]]
+      */
+
+    assertAll(
+        () -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterTranslationRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+    /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
+      */
+
+    assertAll(
+        () -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterRotationTranslationForwardKinematicsKinematics() {
+
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    /*
+    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
+    velocities should be [[17][-10][1]]
+      */
+
+    assertAll(
+        () -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testNormalize() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
+    wheelSpeeds.normalize(5.5);
+
+    double factor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)
+    );
+  }
+
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
new file mode 100644
index 0000000..5ece28b
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumDriveOdometryTest {
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+      new Rotation2d());
+
+  @Test
+  void testMultipleConsecutiveUpdates() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+    var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(secondPose.getX(), 0.0, 0.01),
+        () -> assertEquals(secondPose.getY(), 0.0, 0.01),
+        () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)
+    );
+  }
+
+  @Test
+  void testTwoIterations() {
+    // 5 units/sec  in the x axis (forward)
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
+        () -> assertEquals(0, pose.getY(), 0.01),
+        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void test90degreeTurn() {
+    // This is a 90 degree turn about the point between front left and rear left wheels
+    // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(12.0, pose.getX(), 0.01),
+        () -> assertEquals(12.0, pose.getY(), 0.01),
+        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
+        3.536, 3.536);
+    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, pose.getX(), 0.1),
+        () -> assertEquals(0.00, pose.getY(), 0.1),
+        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+    );
+  }
+
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
new file mode 100644
index 0000000..e9fbcd1
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
@@ -0,0 +1,262 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  @Test
+  void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
+
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testStraightStrafeInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testStraightStrafeForwardKinematics() {
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
+    );
+  }
+
+  @Test
+  void testTurnInPlaceInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    /*
+    The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
+    the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
+    trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
+    our wheels must trace out 1 rotation (or 106.63 inches) per second.
+      */
+
+    assertAll(
+        () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1),
+        () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testTurnInPlaceForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
+    SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
+    SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
+    SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testOffCenterCORRotationInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
+
+    /*
+    This one is a bit trickier. Because we are rotating about the front-left wheel,
+    it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
+    an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
+    radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
+    should be pointing straight forward, the back-left wheel should be pointing straight right,
+    and the back-right wheel should be at a -45 degree angle
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1),
+        () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
+        () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
+        () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1),
+        () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
+    );
+  }
+
+  @Test
+  void testOffCenterCORRotationForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
+    SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
+    SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
+    SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    /*
+    We already know that our omega should be 2pi from the previous test. Next, we need to determine
+    the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
+    we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
+    a full revolution about the center of revolution once every second. Therefore, the center of
+    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
+    1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+    */
+
+    assertAll(
+        () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
+        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual,
+                                 SwerveModuleState tolerance) {
+    assertAll(
+        () -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond,
+            tolerance.speedMetersPerSecond),
+        () -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(),
+            tolerance.angle.getDegrees())
+    );
+  }
+
+  /**
+   * Test the rotation of the robot about a non-central point with
+   * both linear and angular velocities.
+   */
+  @Test
+  void testOffCenterCORRotationAndTranslationInverseKinematics() {
+
+    ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
+
+    // By equation (13.14) from state-space guide, our wheels/angles will be as follows,
+    // (+-1 degree or speed):
+    SwerveModuleState[] expectedStates = new SwerveModuleState[]{
+        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
+        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
+        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
+        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
+    };
+    var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
+
+    for (int i = 0; i < expectedStates.length; i++) {
+      assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
+    }
+  }
+
+  @Test
+  void testOffCenterCORRotationAndTranslationForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
+    SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
+    SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
+    SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    /*
+    From equation (13.17), we know that chassis motion is th dot product of the
+    pseudoinverse of the inverseKinematics matrix (with the center of rotation at
+    (0,0) -- we don't want the motion of the center of rotation, we want it of
+    the center of the robot). These above SwerveModuleStates are known to be from
+    a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
+    calculated using Numpy's linalg.pinv function.
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)
+    );
+  }
+
+  @Test
+  void testNormalize() {
+    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);
+
+    double factor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)
+    );
+  }
+
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
new file mode 100644
index 0000000..f1ee907
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.kinematics;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveDriveOdometryTest {
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+      new Rotation2d());
+
+  @Test
+  void testTwoIterations() {
+    // 5 units/sec  in the x axis (forward)
+    final SwerveModuleState[] wheelSpeeds = {
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+        new SwerveModuleState(5, Rotation2d.fromDegrees(0))
+    };
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(),
+        new SwerveModuleState(), new SwerveModuleState(),
+        new SwerveModuleState(), new SwerveModuleState());
+    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
+        () -> assertEquals(0, pose.getY(), 0.01),
+        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void test90degreeTurn() {
+    // This is a 90 degree turn about the point between front left and rear left wheels
+    //        Module 0: speed 18.84955592153876 angle 90.0
+    //        Module 1: speed 42.14888838624436 angle 26.565051177077986
+    //        Module 2: speed 18.84955592153876 angle -90.0
+    //        Module 3: speed 42.14888838624436 angle -26.565051177077986
+
+    final SwerveModuleState[] wheelSpeeds = {
+        new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
+        new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
+        new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
+        new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
+    };
+    final var zero = new SwerveModuleState();
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
+    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(12.0, pose.getX(), 0.01),
+        () -> assertEquals(12.0, pose.getY(), 0.01),
+        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
+    );
+  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var state = new SwerveModuleState();
+    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
+    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
+    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(1.0, pose.getX(), 0.1),
+        () -> assertEquals(0.00, pose.getY(), 0.1),
+        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
+    );
+  }
+
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java
new file mode 100644
index 0000000..244ca53
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java
@@ -0,0 +1,199 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.math;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.ejml.dense.row.MatrixFeatures_DDRM;
+import org.ejml.simple.SimpleMatrix;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.SimpleMatrixUtils;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class StateSpaceUtilTest {
+  @Test
+  public void testCostArray() {
+    var mat = StateSpaceUtil.makeCostMatrix(
+          VecBuilder.fill(1.0, 2.0, 3.0));
+
+    assertEquals(1.0, mat.get(0, 0), 1e-3);
+    assertEquals(0.0, mat.get(0, 1), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 0), 1e-3);
+    assertEquals(1.0 / 4.0, mat.get(1, 1), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(1.0 / 9.0, mat.get(2, 2), 1e-3);
+  }
+
+  @Test
+  public 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);
+    assertEquals(0.0, mat.get(0, 1), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 0), 1e-3);
+    assertEquals(4.0, mat.get(1, 1), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(9.0, mat.get(2, 2), 1e-3);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testIsStabilizable() {
+    Matrix<N2, N2> A;
+    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
+
+    // First eigenvalue is uncontrollable and unstable.
+    // Second eigenvalue is controllable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and marginally stable.
+    // Second eigenvalue is controllable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and stable.
+    // Second eigenvalue is controllable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
+    assertTrue(StateSpaceUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and stable.
+    // Second eigenvalue is controllable and unstable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
+    assertTrue(StateSpaceUtil.isStabilizable(A, B));
+  }
+
+  @Test
+  public void testMakeWhiteNoiseVector() {
+    var firstData = new ArrayList<Double>();
+    var secondData = new ArrayList<Double>();
+    for (int i = 0; i < 1000; i++) {
+      var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0));
+      firstData.add(noiseVec.get(0, 0));
+      secondData.add(noiseVec.get(1, 0));
+    }
+    assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
+    assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
+  }
+
+  private double calculateStandardDeviation(List<Double> numArray) {
+    double sum = 0.0;
+    double standardDeviation = 0.0;
+    int length = numArray.size();
+
+    for (double num : numArray) {
+      sum += num;
+    }
+
+    double mean = sum / length;
+
+    for (double num : numArray) {
+      standardDeviation += Math.pow(num - mean, 2);
+    }
+
+    return Math.sqrt(standardDeviation / length);
+  }
+
+  @Test
+  public void testDiscretizeA() {
+    var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    var x0 = VecBuilder.fill(1, 1);
+    var discA = Discretization.discretizeA(contA, 1.0);
+    var x1Discrete = discA.times(x0);
+
+    // We now have pos = vel = 1 and accel = 0, which should give us:
+    var x1Truth = VecBuilder.fill(x0.get(0, 0) + 1.0 * x0.get(1, 0),
+          x0.get(1, 0));
+    assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  public void testDiscretizeAB() {
+    var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    var contB = VecBuilder.fill(0, 1);
+    var x0 = VecBuilder.fill(1, 1);
+    var u = VecBuilder.fill(1);
+
+    var abPair = Discretization.discretizeAB(contA, contB, 1.0);
+
+    var x1Discrete = abPair.getFirst().times(x0).plus(abPair.getSecond().times(u));
+
+    // We now have pos = vel = accel = 1, which should give us:
+    var x1Truth = VecBuilder.fill(x0.get(0, 0) + x0.get(1, 0) + 0.5 * u.get(0, 0), x0.get(0, 0)
+          + u.get(0, 0));
+
+    assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
+  }
+
+  @Test
+  public void testMatrixExp() {
+    Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
+    var wrappedResult = wrappedMatrix.exp();
+
+    assertTrue(wrappedResult.isEqual(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
+
+    var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
+    wrappedResult = matrix.times(0.01).exp();
+
+    assertTrue(wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912,
+              0.03076368, 1.04111993), 1E-8));
+  }
+
+  @Test
+  public void testSimpleMatrixExp() {
+    SimpleMatrix matrix = SimpleMatrixUtils.eye(2);
+    var result = SimpleMatrixUtils.exp(matrix);
+
+    assertTrue(MatrixFeatures_DDRM.isIdentical(
+          result.getDDRM(),
+          new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
+          1E-9
+    ));
+
+    matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4});
+    result = SimpleMatrixUtils.exp(matrix.scale(0.01));
+
+    assertTrue(MatrixFeatures_DDRM.isIdentical(
+          result.getDDRM(),
+          new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912,
+              0.03076368, 1.04111993}).getDDRM(),
+          1E-8
+    ));
+  }
+
+  @Test
+  public 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);
+    assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
+    assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
+  }
+
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
new file mode 100644
index 0000000..710bab5
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+
+class CubicHermiteSplineTest {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
+  private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
+    // Start the timer.
+    //var start = System.nanoTime();
+
+    // Generate and parameterize the spline.
+    var controlVectors =
+        SplineHelper.getCubicControlVectorsFromWaypoints(a,
+            waypoints.toArray(new Translation2d[0]), b);
+    var splines
+        = SplineHelper.getCubicSplinesFromControlVectors(
+        controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
+
+    var poses = new ArrayList<PoseWithCurvature>();
+
+    poses.add(splines[0].getPoint(0.0));
+
+    for (var spline : splines) {
+      poses.addAll(SplineParameterizer.parameterize(spline));
+    }
+
+    // End the timer.
+    //var end = System.nanoTime();
+
+    // Calculate the duration (used when benchmarking)
+    //var durationMicroseconds = (end - start) / 1000.0;
+
+    for (int i = 0; i < poses.size() - 1; i++) {
+      var p0 = poses.get(i);
+      var p1 = poses.get(i + 1);
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      var twist = p0.poseMeters.log(p1.poseMeters);
+      assertAll(
+          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
+      );
+    }
+
+    // Check first point
+    assertAll(
+        () -> assertEquals(a.getX(),
+            poses.get(0).poseMeters.getX(), 1E-9),
+        () -> assertEquals(a.getY(),
+            poses.get(0).poseMeters.getY(), 1E-9),
+        () -> assertEquals(a.getRotation().getRadians(),
+            poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
+    );
+
+    // Check interior waypoints
+    boolean interiorsGood = true;
+    for (var waypoint : waypoints) {
+      boolean found = false;
+      for (var state : poses) {
+        if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
+          found = true;
+        }
+      }
+      interiorsGood &= found;
+    }
+
+    assertTrue(interiorsGood);
+
+    // Check last point
+    assertAll(
+        () -> assertEquals(b.getX(),
+            poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
+        () -> assertEquals(b.getY(),
+            poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
+        () -> assertEquals(b.getRotation().getRadians(),
+            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
+    );
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testStraightLine() {
+    run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSCurve() {
+    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
+    ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(1, 1));
+    waypoints.add(new Translation2d(2, -1));
+    var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
+
+    run(start, waypoints, end);
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testOneInterior() {
+    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+    ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(2.0, 0.0));
+    var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
+
+    run(start, waypoints, end);
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testWindyPath() {
+    final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+    final ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(0.5, 0.5));
+    waypoints.add(new Translation2d(0.5, 0.5));
+    waypoints.add(new Translation2d(1.0, 0.0));
+    waypoints.add(new Translation2d(1.5, 0.5));
+    waypoints.add(new Translation2d(2.0, 0.0));
+    waypoints.add(new Translation2d(2.5, 0.5));
+    final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
+
+    run(start, waypoints, end);
+  }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+        new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class, () -> run(
+        new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+        Arrays.asList(new Translation2d(10, 10.5)),
+        new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
new file mode 100644
index 0000000..3b35db0
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.spline;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class QuinticHermiteSplineTest {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
+  private void run(Pose2d a, Pose2d b) {
+    // Start the timer.
+    //var start = System.nanoTime();
+
+    // Generate and parameterize the spline.
+    var spline = SplineHelper.getQuinticSplinesFromWaypoints(List.of(a, b))[0];
+    var poses = SplineParameterizer.parameterize(spline);
+
+    // End the timer.
+    //var end = System.nanoTime();
+
+    // Calculate the duration (used when benchmarking)
+    //var durationMicroseconds = (end - start) / 1000.0;
+
+    for (int i = 0; i < poses.size() - 1; i++) {
+      var p0 = poses.get(i);
+      var p1 = poses.get(i + 1);
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      var twist = p0.poseMeters.log(p1.poseMeters);
+      assertAll(
+          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
+    }
+
+    // Check first point
+    assertAll(
+        () -> assertEquals(
+            a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
+        () -> assertEquals(
+            a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
+        () -> assertEquals(
+            a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
+            1E-9));
+
+    // Check last point
+    assertAll(
+        () -> assertEquals(b.getX(), poses.get(poses.size() - 1)
+            .poseMeters.getX(), 1E-9),
+        () -> assertEquals(b.getY(), poses.get(poses.size() - 1)
+            .poseMeters.getY(), 1E-9),
+        () -> assertEquals(b.getRotation().getRadians(),
+            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testStraightLine() {
+    run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSimpleSCurve() {
+    run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSquiggly() {
+    run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
+        new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
+  }
+
+  @Test
+  void testMalformed() {
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+          new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(MalformedSplineException.class,
+        () -> run(
+          new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+          new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java
new file mode 100644
index 0000000..8af7ef0
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class LinearSystemIDTest {
+  @Test
+  public void testDrivetrainVelocitySystem() {
+    var model = LinearSystemId.createDrivetrainVelocitySystem(
+            DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6
+    );
+    assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(),
+            Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
+
+    assertTrue(model.getB().isEqual(Matrix.mat(Nat.N2(),
+            Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
+
+    assertTrue(model.getC().isEqual(Matrix.mat(Nat.N2(),
+            Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001));
+
+    assertTrue(model.getD().isEqual(Matrix.mat(Nat.N2(),
+            Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001));
+  }
+
+  @Test
+  public 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));
+
+    assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
+
+    assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(),
+            Nat.N2()).fill(1, 0), 0.001));
+
+    assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
+  }
+
+  @Test
+  public void testFlywheelSystem() {
+    var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
+    assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
+
+    assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
+
+    assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
+
+    assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
+  }
+
+  @Test
+  public void testIdentifyPositionSystem() {
+    // By controls engineering in frc,
+    // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
+    var kv = 1.0;
+    var ka = 0.5;
+    var model = LinearSystemId.identifyPositionSystem(kv, ka);
+
+    assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka));
+    assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
+  }
+
+  @Test
+  public void testIdentifyVelocitySystem() {
+    // By controls engineering in frc,
+    // V = kv * velocity + ka * acceleration
+    // x-dot =  -kv/ka * v + 1/ka \cdot V
+    var kv = 1.0;
+    var ka = 0.5;
+    var model = LinearSystemId.identifyVelocitySystem(kv, ka);
+
+    assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
+    assertEquals(model.getB(), VecBuilder.fill(1 / ka));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java
new file mode 100644
index 0000000..de39295
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.system;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+import edu.wpi.first.wpiutil.math.Nat;
+import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpiutil.math.numbers.N1;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class RungeKuttaTest {
+  @Test
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public void testExponential() {
+
+    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
+
+    //noinspection SuspiciousNameCombination
+    var y1 = RungeKutta.rungeKutta((Matrix<N1, N1> x) -> {
+      var y = new Matrix<>(Nat.N1(), Nat.N1());
+      y.set(0, 0, Math.exp(x.get(0, 0)));
+      return y; },
+            y0, 0.1
+    );
+
+    assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
new file mode 100644
index 0000000..0c2f4f1
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class CentripetalAccelerationConstraintTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCentripetalAccelerationConstraint() {
+    double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
+    var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
+
+    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+        Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var centripetalAcceleration
+          = Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
+
+      t += dt;
+      assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
+    }
+  }
+
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
new file mode 100644
index 0000000..6ed9966
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDriveKinematicsConstraintTest {
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  @Test
+  void testDifferentialDriveKinematicsConstraint() {
+    double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
+    var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
+
+    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+        Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var chassisSpeeds = new ChassisSpeeds(
+          point.velocityMetersPerSecond, 0,
+          point.velocityMetersPerSecond * point.curvatureRadPerMeter
+      );
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      t += dt;
+      assertAll(
+          () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
+          () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)
+      );
+    }
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
new file mode 100644
index 0000000..fff4c61
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.Collections;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class DifferentialDriveVoltageConstraintTest {
+  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
+  @Test
+  void testDifferentialDriveVoltageConstraint() {
+    // Pick an unreasonably large kA to ensure the constraint has to do some work
+    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
+    var kinematics = new DifferentialDriveKinematics(0.5);
+    double maxVoltage = 10;
+    var constraint = new DifferentialDriveVoltageConstraint(feedforward,
+                                                            kinematics,
+                                                            maxVoltage);
+
+    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
+        Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var chassisSpeeds = new ChassisSpeeds(
+          point.velocityMetersPerSecond, 0,
+          point.velocityMetersPerSecond * point.curvatureRadPerMeter
+      );
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      t += dt;
+
+      // Not really a strictly-correct test as we're using the chassis accel instead of the
+      // wheel accel, but much easier than doing it "properly" and a reasonable check anyway
+      assertAll(
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               <= maxVoltage + 0.05),
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               >= -maxVoltage - 0.05),
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               <= maxVoltage + 0.05),
+          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
+                                                 point.accelerationMetersPerSecondSq)
+                               >= -maxVoltage - 0.05)
+      );
+    }
+  }
+
+  @Test
+  void testEndpointHighCurvature() {
+    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
+
+    // Large trackwidth - need to test with radius of curvature less than half of trackwidth
+    var kinematics = new DifferentialDriveKinematics(3);
+    double maxVoltage = 10;
+    var constraint = new DifferentialDriveVoltageConstraint(feedforward,
+                                                            kinematics,
+                                                            maxVoltage);
+
+    var config = new TrajectoryConfig(12, 12).addConstraint(constraint);
+
+    // Radius of curvature should be ~1 meter.
+    assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
+        new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
+        new ArrayList<Translation2d>(),
+        new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
+        config));
+
+    assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
+        new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
+        new ArrayList<Translation2d>(),
+        new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
+        config.setReversed(true)));
+
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java
new file mode 100644
index 0000000..513db06
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.constraint.EllipticalRegionConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class EllipticalRegionConstraintTest {
+  @Test
+  void testConstraint() {
+    // Create constraints
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+    var regionConstraint = new EllipticalRegionConstraint(
+        new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)),
+        Units.feetToMeters(10.0), Units.feetToMeters(5.0), Rotation2d.fromDegrees(180.0),
+        maxVelocityConstraint
+    );
+
+    // Get trajectory
+    var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
+
+    // Iterate through trajectory and check constraints
+    boolean exceededConstraintOutsideRegion = false;
+    for (var point : trajectory.getStates()) {
+      var translation = point.poseMeters.getTranslation();
+
+      if (translation.getX() < Units.feetToMeters(10)
+          && translation.getY() < Units.feetToMeters(5)) {
+        assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
+      } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
+        exceededConstraintOutsideRegion = true;
+      }
+    }
+    assertTrue(exceededConstraintOutsideRegion);
+  }
+
+  @Test
+  void testIsPoseWithinRegion() {
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+
+    var regionConstraintNoRotation = new EllipticalRegionConstraint(
+        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+        Units.feetToMeters(2.0), Units.feetToMeters(4.0), new Rotation2d(),
+        maxVelocityConstraint);
+
+    assertFalse(regionConstraintNoRotation.isPoseInRegion(new Pose2d(
+        Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
+    )));
+
+    var regionConstraintWithRotation = new EllipticalRegionConstraint(
+        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+        Units.feetToMeters(2.0), Units.feetToMeters(4.0), Rotation2d.fromDegrees(90.0),
+        maxVelocityConstraint);
+
+    assertTrue(regionConstraintWithRotation.isPoseInRegion(new Pose2d(
+        Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
+    )));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java
new file mode 100644
index 0000000..94eeb35
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
+import edu.wpi.first.wpilibj.trajectory.constraint.RectangularRegionConstraint;
+import edu.wpi.first.wpilibj.util.Units;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class RectangularRegionConstraintTest {
+  @Test
+  void testConstraint() {
+    // Create constraints
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+    var regionConstraint = new RectangularRegionConstraint(
+        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+        new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
+        maxVelocityConstraint
+    );
+
+    // Get trajectory
+    var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
+
+    // Iterate through trajectory and check constraints
+    boolean exceededConstraintOutsideRegion = false;
+    for (var point : trajectory.getStates()) {
+      if (regionConstraint.isPoseInRegion(point.poseMeters)) {
+        assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
+      } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
+        exceededConstraintOutsideRegion = true;
+      }
+    }
+    assertTrue(exceededConstraintOutsideRegion);
+  }
+
+  @Test
+  void testIsPoseWithinRegion() {
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+    var regionConstraint = new RectangularRegionConstraint(
+        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+        new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
+        maxVelocityConstraint
+    );
+
+    assertFalse(regionConstraint.isPoseInRegion(new Pose2d()));
+    assertTrue(regionConstraint.isPoseInRegion(new Pose2d(Units.feetToMeters(3.0),
+        Units.feetToMeters(14.5), new Rotation2d())));
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
new file mode 100644
index 0000000..2bfa972
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+
+import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+class TrajectoryGeneratorTest {
+  static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
+    final double maxVelocity = feetToMeters(12.0);
+    final double maxAccel = feetToMeters(12);
+
+    // 2018 cross scale auto waypoints.
+    var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23),
+        Rotation2d.fromDegrees(-180));
+    var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8),
+        Rotation2d.fromDegrees(-160));
+
+    var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(sideStart);
+    waypoints.add(sideStart.plus(
+        new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)),
+            new Rotation2d())));
+    waypoints.add(sideStart.plus(
+        new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
+            Rotation2d.fromDegrees(-90))));
+    waypoints.add(crossScale);
+
+    TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
+        .setReversed(true)
+        .addConstraints(constraints);
+
+    return TrajectoryGenerator.generateTrajectory(waypoints, config);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  void testGenerationAndConstraints() {
+    Trajectory trajectory = getTrajectory(new ArrayList<>());
+
+    double duration = trajectory.getTotalTimeSeconds();
+    double t = 0.0;
+    double dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      t += dt;
+      assertAll(
+          () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
+          () -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0)
+              + 0.05)
+      );
+    }
+  }
+
+  @Test
+  void testMalformedTrajectory() {
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+          Arrays.asList(
+            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+            new Pose2d(1, 0, Rotation2d.fromDegrees(180))
+          ),
+          new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
+        );
+
+    assertEquals(traj.getStates().size(), 1);
+    assertEquals(traj.getTotalTimeSeconds(), 0);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
new file mode 100644
index 0000000..d8d59b1
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+public class TrajectoryJsonTest {
+  @Test
+  void deserializeMatches() {
+    var config = List.of(new DifferentialDriveKinematicsConstraint(
+        new DifferentialDriveKinematics(20), 3));
+    var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
+
+    var deserialized =
+        assertDoesNotThrow(() ->
+            TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory)));
+
+    assertEquals(trajectory.getStates(), deserialized.getStates());
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
new file mode 100644
index 0000000..b17046b
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class TrajectoryTransformTest {
+  @Test
+  void testTransformBy() {
+    var config = new TrajectoryConfig(3, 3);
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+        new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
+        config
+    );
+
+    var transformedTrajectory = trajectory.transformBy(
+        new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
+
+    // Test initial pose.
+    assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
+        transformedTrajectory.sample(0).poseMeters);
+
+    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+  }
+
+  @Test
+  void testRelativeTo() {
+    var config = new TrajectoryConfig(3, 3);
+    var trajectory = TrajectoryGenerator.generateTrajectory(
+        new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
+        List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
+        config
+    );
+
+    var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
+
+    // Test initial pose.
+    assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
+
+    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+  }
+
+  void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
+    for (int i = 0; i < statesA.size() - 1; i++) {
+      var a1 = statesA.get(i).poseMeters;
+      var a2 = statesA.get(i + 1).poseMeters;
+
+      var b1 = statesB.get(i).poseMeters;
+      var b2 = statesB.get(i + 1).poseMeters;
+
+      assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
+    }
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
new file mode 100644
index 0000000..e155188
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
@@ -0,0 +1,257 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+class TrapezoidProfileTest {
+  private static final double kDt = 0.01;
+
+  /**
+   * Asserts "val1" is less than or equal to "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   */
+  private static void assertLessThanOrEquals(double val1, double val2) {
+    assertTrue(val1 <= val2, val1 + " is greater than " + val2);
+  }
+
+  /**
+   * Asserts "val1" is within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertNear(double val1, double val2, double eps) {
+    assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
+        + " is greater than " + eps);
+  }
+
+  /**
+   * Asserts "val1" is less than or within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertLessThanOrNear(double val1, double val2, double eps) {
+    if (val1 <= val2) {
+      assertLessThanOrEquals(val1, val2);
+    } else {
+      assertNear(val1, val2, eps);
+    }
+  }
+
+  @Test
+  void reachesGoal() {
+    TrapezoidProfile.Constraints constraints =
+        new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 450; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Tests that decreasing the maximum velocity in the middle when it is already
+  // moving faster than the new max is handled correctly
+  @Test
+  void posContinousUnderVelChange() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double lastPos = state.position;
+    for (int i = 0; i < 1600; ++i) {
+      if (i == 400) {
+        constraints.maxVelocity = 0.75;
+      }
+
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      double estimatedVel = (state.position - lastPos) / kDt;
+
+      if (i >= 400) {
+        // Since estimatedVel can have floating point rounding errors, we check
+        // whether value is less than or within an error delta of the new
+        // constraint.
+        assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
+
+        assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
+      }
+
+      lastPos = state.position;
+    }
+    assertEquals(state, goal);
+  }
+
+  // There is some somewhat tricky code for dealing with going backwards
+  @Test
+  void backwards() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void switchGoalInMiddle() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNotEquals(state, goal);
+
+    goal = new TrapezoidProfile.State(0.0, 0.0);
+    for (int i = 0; i < 550; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Checks to make sure that it hits top speed
+  @Test
+  void topSpeed() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNear(constraints.maxVelocity, state.velocity, 10e-5);
+
+    for (int i = 0; i < 2000; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void timingToCurrent() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; i++) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
+    }
+  }
+
+  @Test
+  void timingToGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(1);
+    boolean reachedGoal = false;
+    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)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingToNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(-1);
+    boolean reachedGoal = false;
+    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)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
new file mode 100644
index 0000000..9938660
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class UnitsTest extends UtilityClassTest<Units> {
+  UnitsTest() {
+    super(Units.class);
+  }
+
+  @Test
+  void metersToFeetTest() {
+    assertEquals(3.28, Units.metersToFeet(1), 1e-2);
+  }
+
+  @Test
+  void feetToMetersTest() {
+    assertEquals(0.30, Units.feetToMeters(1), 1e-2);
+  }
+
+  @Test
+  void metersToInchesTest() {
+    assertEquals(39.37, Units.metersToInches(1), 1e-2);
+  }
+
+  @Test
+  void inchesToMetersTest() {
+    assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
+  }
+
+  @Test
+  void degreesToRadiansTest() {
+    assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
+  }
+
+  @Test
+  void radiansToDegreesTest() {
+    assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
+  }
+
+  @Test
+  void rotationsPerMinuteToRadiansPerSecondTest() {
+    assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
+  }
+
+  @Test
+  void radiansPerSecondToRotationsPerMinute() {
+    assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java
new file mode 100644
index 0000000..14a0f7c
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MathUtilTest {
+  @Test
+  void testAngleNormalize() {
+    assertEquals(MathUtil.normalizeAngle(5 * Math.PI), Math.PI);
+    assertEquals(MathUtil.normalizeAngle(-5 * Math.PI), Math.PI);
+    assertEquals(MathUtil.normalizeAngle(Math.PI / 2), Math.PI / 2);
+    assertEquals(MathUtil.normalizeAngle(-Math.PI / 2), -Math.PI / 2);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java

diff --git a/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java
new file mode 100644
index 0000000..1af0625
--- /dev/null
+++ b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java
@@ -0,0 +1,174 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil.math;
+
+import org.ejml.data.SingularMatrixException;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpiutil.math.numbers.N3;
+import edu.wpi.first.wpiutil.math.numbers.N4;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+public class MatrixTest {
+  @Test
+  void testMatrixMultiplication() {
+    var mat1 = Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(2.0, 1.0,
+            0.0, 1.0);
+    var mat2 = Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(3.0, 0.0,
+            0.0, 2.5);
+
+    Matrix<N2, N2> result = mat1.times(mat2);
+
+    assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5));
+
+    var mat3 = Matrix.mat(Nat.N2(), Nat.N3())
+        .fill(1.0, 3.0, 0.5,
+            2.0, 4.3, 1.2);
+    var mat4 = Matrix.mat(Nat.N3(), Nat.N4())
+        .fill(3.0, 1.5, 2.0, 4.5,
+            2.3, 1.0, 1.6, 3.1,
+            5.2, 2.1, 2.0, 1.0);
+
+    Matrix<N2, N4> result2 = mat3.times(mat4);
+
+    assertTrue(Matrix.mat(Nat.N2(), Nat.N4())
+        .fill(12.5, 5.55, 7.8, 14.3,
+            22.13, 9.82, 13.28, 23.53).isEqual(
+            result2,
+              1E-9
+    ));
+  }
+
+  @Test
+  void testMatrixVectorMultiplication() {
+    var mat = Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(1.0, 1.0,
+            0.0, 1.0);
+
+    var vec = VecBuilder.fill(3.0, 2.0);
+
+    Matrix<N2, N1> result = mat.times(vec);
+    assertEquals(VecBuilder.fill(5.0, 2.0), result);
+  }
+
+  @Test
+  void testTranspose() {
+    Matrix<N3, N1> vec = VecBuilder
+        .fill(1.0,
+            2.0,
+            3.0);
+
+    Matrix<N1, N3> transpose = vec.transpose();
+
+    assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose);
+  }
+
+  @Test
+  void testSolve() {
+    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
+    var vec1 = VecBuilder.fill(1.0, 2.0);
+
+    var solve1 = mat1.solve(vec1);
+
+    assertEquals(VecBuilder.fill(0.0, 0.5), solve1);
+
+    var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
+    var vec2 = VecBuilder.fill(1.0, 2.0, 3.0);
+
+    var solve2 = mat2.solve(vec2);
+
+    assertEquals(VecBuilder.fill(0.0, 0.5), solve2);
+  }
+
+  @Test
+  void testInverse() {
+    var mat = Matrix.mat(Nat.N3(), Nat.N3())
+        .fill(1.0, 3.0, 2.0,
+            5.0, 2.0, 1.5,
+            0.0, 1.3, 2.5);
+
+    var inv = mat.inv();
+
+    assertTrue(Matrix.eye(Nat.N3()).isEqual(
+        mat.times(inv),
+        1E-9
+    ));
+
+    assertTrue(Matrix.eye(Nat.N3()).isEqual(
+        inv.times(mat),
+        1E-9
+    ));
+  }
+
+  @Test
+  void testUninvertableMatrix() {
+    var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(2.0, 1.0,
+            2.0, 1.0);
+
+    assertThrows(SingularMatrixException.class, singularMatrix::inv);
+  }
+
+  @Test
+  void testMatrixScalarArithmetic() {
+    var mat = Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(1.0, 2.0,
+            3.0, 4.0);
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0),  mat.times(2.0));
+
+    assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(
+        mat.div(2.0),
+        1E-3
+    ));
+  }
+
+  @Test
+  void testMatrixMatrixArithmetic() {
+    var mat1 = Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(1.0, 2.0,
+            3.0, 4.0);
+
+    var mat2 = Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(5.0, 6.0,
+            7.0, 8.0);
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0),
+        mat1.minus(mat2)
+    );
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0),
+        mat1.plus(mat2)
+    );
+  }
+
+  @Test
+  void testMatrixExponential() {
+    var matrix = Matrix.eye(Nat.N2());
+    var result = matrix.exp();
+
+    assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
+
+    matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
+    result = matrix.times(0.01).exp();
+
+    assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912,
+        0.03076368, 1.04111993), 1E-8));
+  }
+}

wpimath/src/test/native/cpp/EigenTest.cpp

diff --git a/wpimath/src/test/native/cpp/EigenTest.cpp b/wpimath/src/test/native/cpp/EigenTest.cpp
new file mode 100644
index 0000000..2065e26
--- /dev/null
+++ b/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Eigen/Core"
+#include "Eigen/LU"
+#include "gtest/gtest.h"
+
+TEST(EigenTest, MultiplicationTest) {
+  Eigen::Matrix<double, 2, 2> m1;
+  m1 << 2, 1, 0, 1;
+
+  Eigen::Matrix<double, 2, 2> m2;
+  m2 << 3, 0, 0, 2.5;
+
+  const auto result = m1 * m2;
+
+  Eigen::Matrix<double, 2, 2> expectedResult;
+  expectedResult << 6.0, 2.5, 0.0, 2.5;
+
+  EXPECT_TRUE(expectedResult.isApprox(result));
+
+  Eigen::Matrix<double, 2, 3> m3;
+  m3 << 1.0, 3.0, 0.5, 2.0, 4.3, 1.2;
+
+  Eigen::Matrix<double, 3, 4> m4;
+  m4 << 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0;
+
+  const auto result2 = m3 * m4;
+
+  Eigen::Matrix<double, 2, 4> expectedResult2;
+  expectedResult2 << 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53;
+
+  EXPECT_TRUE(expectedResult2.isApprox(result2));
+}
+
+TEST(EigenTest, TransposeTest) {
+  Eigen::Matrix<double, 3, 1> vec;
+  vec << 1, 2, 3;
+
+  const auto transpose = vec.transpose();
+
+  Eigen::Matrix<double, 1, 3> expectedTranspose;
+  expectedTranspose << 1, 2, 3;
+
+  EXPECT_TRUE(expectedTranspose.isApprox(transpose));
+}
+
+TEST(EigenTest, InverseTest) {
+  Eigen::Matrix<double, 3, 3> mat;
+  mat << 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5;
+
+  const auto inverse = mat.inverse();
+  const auto identity = Eigen::MatrixXd::Identity(3, 3);
+
+  EXPECT_TRUE(identity.isApprox(mat * inverse));
+}

wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp

diff --git a/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp
new file mode 100644
index 0000000..934e14b
--- /dev/null
+++ b/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h"  // NOLINT(build/include_order)
+
+#include <cmath>
+#include <memory>
+#include <random>
+
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+// Filter constants
+static constexpr units::second_t kFilterStep = 0.005_s;
+static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr int32_t kMovAvgTaps = 6;
+
+enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
+
+std::ostream& operator<<(std::ostream& os,
+                         const LinearFilterNoiseTestType& type) {
+  switch (type) {
+    case TEST_SINGLE_POLE_IIR:
+      os << "LinearFilter SinglePoleIIR";
+      break;
+    case TEST_MOVAVG:
+      os << "LinearFilter MovingAverage";
+      break;
+  }
+
+  return os;
+}
+
+static double GetData(double t) {
+  return 100.0 * std::sin(2.0 * wpi::math::pi * t);
+}
+
+class LinearFilterNoiseTest
+    : public testing::TestWithParam<LinearFilterNoiseTestType> {
+ protected:
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
+
+  void SetUp() override {
+    switch (GetParam()) {
+      case TEST_SINGLE_POLE_IIR: {
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
+        break;
+      }
+
+      case TEST_MOVAVG: {
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+        break;
+      }
+    }
+  }
+};
+
+/**
+ * Test if the filter reduces the noise produced by a signal generator
+ */
+TEST_P(LinearFilterNoiseTest, NoiseReduce) {
+  double noiseGenError = 0.0;
+  double filterError = 0.0;
+
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+  std::normal_distribution<double> distr{0.0, 10.0};
+
+  for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+    double theory = GetData(t.to<double>());
+    double noise = distr(gen);
+    filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
+    noiseGenError += std::abs(noise - theory);
+  }
+
+  RecordProperty("FilterError", filterError);
+
+  // The filter should have produced values closer to the theory
+  EXPECT_GT(noiseGenError, filterError)
+      << "Filter should have reduced noise accumulation but failed";
+}
+
+INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
+                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));

wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp

diff --git a/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp
new file mode 100644
index 0000000..d321518
--- /dev/null
+++ b/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h"  // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+// Filter constants
+static constexpr units::second_t kFilterStep = 0.005_s;
+static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
+static constexpr double kHighPassTimeConstant = 0.006631;
+static constexpr double kHighPassExpectedOutput = 10.074717;
+static constexpr int32_t kMovAvgTaps = 6;
+static constexpr double kMovAvgExpectedOutput = -10.191644;
+
+enum LinearFilterOutputTestType {
+  TEST_SINGLE_POLE_IIR,
+  TEST_HIGH_PASS,
+  TEST_MOVAVG,
+  TEST_PULSE
+};
+
+std::ostream& operator<<(std::ostream& os,
+                         const LinearFilterOutputTestType& type) {
+  switch (type) {
+    case TEST_SINGLE_POLE_IIR:
+      os << "LinearFilter SinglePoleIIR";
+      break;
+    case TEST_HIGH_PASS:
+      os << "LinearFilter HighPass";
+      break;
+    case TEST_MOVAVG:
+      os << "LinearFilter MovingAverage";
+      break;
+    case TEST_PULSE:
+      os << "LinearFilter Pulse";
+      break;
+  }
+
+  return os;
+}
+
+static double GetData(double t) {
+  return 100.0 * std::sin(2.0 * wpi::math::pi * t) +
+         20.0 * std::cos(50.0 * wpi::math::pi * t);
+}
+
+static double GetPulseData(double t) {
+  if (std::abs(t - 1.0) < 0.001) {
+    return 1.0;
+  } else {
+    return 0.0;
+  }
+}
+
+/**
+ * A fixture that includes a consistent data source wrapped in a filter
+ */
+class LinearFilterOutputTest
+    : public testing::TestWithParam<LinearFilterOutputTestType> {
+ protected:
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
+  std::function<double(double)> m_data;
+  double m_expectedOutput = 0.0;
+
+  void SetUp() override {
+    switch (GetParam()) {
+      case TEST_SINGLE_POLE_IIR: {
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
+        m_data = GetData;
+        m_expectedOutput = kSinglePoleIIRExpectedOutput;
+        break;
+      }
+
+      case TEST_HIGH_PASS: {
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+                                                kFilterStep));
+        m_data = GetData;
+        m_expectedOutput = kHighPassExpectedOutput;
+        break;
+      }
+
+      case TEST_MOVAVG: {
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+        m_data = GetData;
+        m_expectedOutput = kMovAvgExpectedOutput;
+        break;
+      }
+
+      case TEST_PULSE: {
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+        m_data = GetPulseData;
+        m_expectedOutput = 0.0;
+        break;
+      }
+    }
+  }
+};
+
+/**
+ * Test if the linear filters produce consistent output for a given data set.
+ */
+TEST_P(LinearFilterOutputTest, Output) {
+  double filterOutput = 0.0;
+  for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+    filterOutput = m_filter->Calculate(m_data(t.to<double>()));
+  }
+
+  RecordProperty("LinearFilterOutput", filterOutput);
+
+  EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+      << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
+                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
+                                         TEST_MOVAVG, TEST_PULSE));

wpimath/src/test/native/cpp/MedianFilterTest.cpp

diff --git a/wpimath/src/test/native/cpp/MedianFilterTest.cpp b/wpimath/src/test/native/cpp/MedianFilterTest.cpp
new file mode 100644
index 0000000..2a02e1c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/MedianFilterTest.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/MedianFilter.h"
+#include "gtest/gtest.h"
+
+TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+
+  EXPECT_EQ(filter.Calculate(1000), 3.5);
+}
+
+TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+  filter.Calculate(7);
+
+  EXPECT_EQ(filter.Calculate(1000), 4);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestEven) {
+  frc::MedianFilter<double> filter{6};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 4.5);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestOdd) {
+  frc::MedianFilter<double> filter{5};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 5);
+}

wpimath/src/test/native/cpp/StateSpaceTest.cpp

diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
new file mode 100644
index 0000000..a1600fc
--- /dev/null
+++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <random>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/estimator/KalmanFilter.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/LinearSystemLoop.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/time.h"
+
+namespace frc {
+
+constexpr double kPositionStddev = 0.0001;
+constexpr auto kDt = 0.00505_s;
+
+class StateSpace : public testing::Test {
+ public:
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(2);
+
+    // Carriage mass
+    constexpr auto m = 5_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.0181864_m;
+
+    // Gear ratio
+    constexpr double G = 40.0 / 40.0;
+
+    return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+  }();
+  LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt};
+  KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt};
+  LinearSystemLoop<2, 1, 1> loop{plant, controller, observer, 12_V, kDt};
+};
+
+void Update(LinearSystemLoop<2, 1, 1>& loop, double noise) {
+  Eigen::Matrix<double, 1, 1> y =
+      loop.Plant().CalculateY(loop.Xhat(), loop.U()) +
+      Eigen::Matrix<double, 1, 1>(noise);
+  loop.Correct(y);
+  loop.Predict(kDt);
+}
+
+TEST_F(StateSpace, CorrectPredictLoop) {
+  std::default_random_engine generator;
+  std::normal_distribution<double> dist{0.0, kPositionStddev};
+
+  Eigen::Matrix<double, 2, 1> references;
+  references << 2.0, 0.0;
+  loop.SetNextR(references);
+
+  for (int i = 0; i < 1000; i++) {
+    Update(loop, dist(generator));
+    EXPECT_PRED_FORMAT2(testing::DoubleLE, -12.0, loop.U(0));
+    EXPECT_PRED_FORMAT2(testing::DoubleLE, loop.U(0), 12.0);
+  }
+
+  EXPECT_NEAR(loop.Xhat(0), 2.0, 0.05);
+  EXPECT_NEAR(loop.Xhat(1), 0.0, 0.5);
+}
+
+}  // namespace frc

wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp

diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
new file mode 100644
index 0000000..24e9cf2
--- /dev/null
+++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <array>
+
+#include "Eigen/Core"
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/RungeKutta.h"
+
+TEST(StateSpaceUtilTest, MakeMatrix) {
+  // Column vector
+  Eigen::Matrix<double, 2, 1> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
+  EXPECT_NEAR(mat1(0), 1.0, 1e-3);
+  EXPECT_NEAR(mat1(1), 2.0, 1e-3);
+
+  // Row vector
+  Eigen::Matrix<double, 1, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
+  EXPECT_NEAR(mat2(0), 1.0, 1e-3);
+  EXPECT_NEAR(mat2(1), 2.0, 1e-3);
+
+  // Square matrix
+  Eigen::Matrix<double, 2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0);
+  EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3);
+  EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3);
+  EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3);
+  EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3);
+
+  // Nonsquare matrix with more rows than columns
+  Eigen::Matrix<double, 3, 2> mat4 =
+      frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
+  EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3);
+  EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3);
+  EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3);
+  EXPECT_NEAR(mat4(1, 1), 4.0, 1e-3);
+  EXPECT_NEAR(mat4(2, 0), 5.0, 1e-3);
+  EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3);
+
+  // Nonsquare matrix with more columns than rows
+  Eigen::Matrix<double, 2, 3> mat5 =
+      frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
+  EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3);
+  EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3);
+  EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3);
+  EXPECT_NEAR(mat5(1, 0), 4.0, 1e-3);
+  EXPECT_NEAR(mat5(1, 1), 5.0, 1e-3);
+  EXPECT_NEAR(mat5(1, 2), 6.0, 1e-3);
+}
+
+TEST(StateSpaceUtilTest, CostParameterPack) {
+  Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
+  EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
+  EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
+}
+
+TEST(StateSpaceUtilTest, CostArray) {
+  Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
+  EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
+  EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
+}
+
+TEST(StateSpaceUtilTest, CovParameterPack) {
+  Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
+  EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
+  EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 1), 4.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
+}
+
+TEST(StateSpaceUtilTest, CovArray) {
+  Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
+  EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
+  EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 1), 4.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
+  EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
+}
+
+TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
+  Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
+  static_cast<void>(vec);
+}
+
+TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
+  Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
+  static_cast<void>(vec);
+}
+
+TEST(StateSpaceUtilTest, IsStabilizable) {
+  Eigen::Matrix<double, 2, 2> A;
+  Eigen::Matrix<double, 2, 1> B;
+  B << 0, 1;
+
+  // We separate the result of IsStabilizable from the assertion because
+  // templates break gtest.
+
+  // First eigenvalue is uncontrollable and unstable.
+  // Second eigenvalue is controllable and stable.
+  A << 1.2, 0, 0, 0.5;
+  bool ret = frc::IsStabilizable<2, 1>(A, B);
+  EXPECT_FALSE(ret);
+
+  // First eigenvalue is uncontrollable and marginally stable.
+  // Second eigenvalue is controllable and stable.
+  A << 1, 0, 0, 0.5;
+  ret = frc::IsStabilizable<2, 1>(A, B);
+  EXPECT_FALSE(ret);
+
+  // First eigenvalue is uncontrollable and stable.
+  // Second eigenvalue is controllable and stable.
+  A << 0.2, 0, 0, 0.5;
+  ret = frc::IsStabilizable<2, 1>(A, B);
+  EXPECT_TRUE(ret);
+
+  // First eigenvalue is uncontrollable and stable.
+  // Second eigenvalue is controllable and unstable.
+  A << 0.2, 0, 0, 1.2;
+  ret = frc::IsStabilizable<2, 1>(A, B);
+  EXPECT_TRUE(ret);
+}

wpimath/src/test/native/cpp/UnitsTest.cpp

diff --git a/wpimath/src/test/native/cpp/UnitsTest.cpp b/wpimath/src/test/native/cpp/UnitsTest.cpp
new file mode 100644
index 0000000..54e8195
--- /dev/null
+++ b/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -0,0 +1,3422 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <array>
+#include <chrono>
+#include <string>
+#include <type_traits>
+
+#include "gtest/gtest.h"
+#include "units/acceleration.h"
+#include "units/angle.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.h"
+#include "units/area.h"
+#include "units/capacitance.h"
+#include "units/charge.h"
+#include "units/concentration.h"
+#include "units/conductance.h"
+#include "units/constants.h"
+#include "units/current.h"
+#include "units/data.h"
+#include "units/data_transfer_rate.h"
+#include "units/density.h"
+#include "units/dimensionless.h"
+#include "units/energy.h"
+#include "units/force.h"
+#include "units/frequency.h"
+#include "units/illuminance.h"
+#include "units/impedance.h"
+#include "units/inductance.h"
+#include "units/length.h"
+#include "units/luminous_flux.h"
+#include "units/luminous_intensity.h"
+#include "units/magnetic_field_strength.h"
+#include "units/magnetic_flux.h"
+#include "units/mass.h"
+#include "units/math.h"
+#include "units/power.h"
+#include "units/pressure.h"
+#include "units/radiation.h"
+#include "units/solid_angle.h"
+#include "units/substance.h"
+#include "units/temperature.h"
+#include "units/time.h"
+#include "units/torque.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+#include "units/volume.h"
+
+using namespace units::acceleration;
+using namespace units::angle;
+using namespace units::angular_acceleration;
+using namespace units::angular_velocity;
+using namespace units::area;
+using namespace units::capacitance;
+using namespace units::charge;
+using namespace units::concentration;
+using namespace units::conductance;
+using namespace units::data;
+using namespace units::data_transfer_rate;
+using namespace units::density;
+using namespace units::dimensionless;
+using namespace units::energy;
+using namespace units::frequency;
+using namespace units::illuminance;
+using namespace units::impedance;
+using namespace units::inductance;
+using namespace units::length;
+using namespace units::luminous_flux;
+using namespace units::luminous_intensity;
+using namespace units::magnetic_field_strength;
+using namespace units::magnetic_flux;
+using namespace units::mass;
+using namespace units::math;
+using namespace units::power;
+using namespace units::pressure;
+using namespace units::radiation;
+using namespace units::solid_angle;
+using namespace units::temperature;
+using namespace units::time;
+using namespace units::torque;
+using namespace units::velocity;
+using namespace units::voltage;
+using namespace units::volume;
+using namespace units;
+
+#if !defined(_MSC_VER) || _MSC_VER > 1800
+using namespace units::literals;
+#endif
+
+namespace {
+
+class TypeTraits : public ::testing::Test {
+ protected:
+  TypeTraits() {}
+  virtual ~TypeTraits() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+class UnitManipulators : public ::testing::Test {
+ protected:
+  UnitManipulators() {}
+  virtual ~UnitManipulators() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+class UnitContainer : public ::testing::Test {
+ protected:
+  UnitContainer() {}
+  virtual ~UnitContainer() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+class UnitConversion : public ::testing::Test {
+ protected:
+  UnitConversion() {}
+  virtual ~UnitConversion() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+class UnitMath : public ::testing::Test {
+ protected:
+  UnitMath() {}
+  virtual ~UnitMath() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+class CompileTimeArithmetic : public ::testing::Test {
+ protected:
+  CompileTimeArithmetic() {}
+  virtual ~CompileTimeArithmetic() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+class Constexpr : public ::testing::Test {
+ protected:
+  Constexpr() {}
+  virtual ~Constexpr() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+class CaseStudies : public ::testing::Test {
+ protected:
+  CaseStudies() {}
+  virtual ~CaseStudies() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+
+  struct RightTriangle {
+    using a = unit_value_t<meters, 3>;
+    using b = unit_value_t<meters, 4>;
+    using c = unit_value_sqrt<
+        unit_value_add<unit_value_power<a, 2>, unit_value_power<b, 2>>>;
+  };
+};
+}  // namespace
+
+TEST_F(TypeTraits, isRatio) {
+  EXPECT_TRUE(traits::is_ratio<std::ratio<1>>::value);
+  EXPECT_FALSE(traits::is_ratio<double>::value);
+}
+
+TEST_F(TypeTraits, ratio_sqrt) {
+  using rt2 = ratio_sqrt<std::ratio<2>>;
+  EXPECT_LT(std::abs(std::sqrt(2 / static_cast<double>(1)) -
+                     rt2::num / static_cast<double>(rt2::den)),
+            5e-9);
+
+  using rt4 = ratio_sqrt<std::ratio<4>>;
+  EXPECT_LT(std::abs(std::sqrt(4 / static_cast<double>(1)) -
+                     rt4::num / static_cast<double>(rt4::den)),
+            5e-9);
+
+  using rt10 = ratio_sqrt<std::ratio<10>>;
+  EXPECT_LT(std::abs(std::sqrt(10 / static_cast<double>(1)) -
+                     rt10::num / static_cast<double>(rt10::den)),
+            5e-9);
+
+  using rt30 = ratio_sqrt<std::ratio<30>>;
+  EXPECT_LT(std::abs(std::sqrt(30 / static_cast<double>(1)) -
+                     rt30::num / static_cast<double>(rt30::den)),
+            5e-9);
+
+  using rt61 = ratio_sqrt<std::ratio<61>>;
+  EXPECT_LT(std::abs(std::sqrt(61 / static_cast<double>(1)) -
+                     rt61::num / static_cast<double>(rt61::den)),
+            5e-9);
+
+  using rt100 = ratio_sqrt<std::ratio<100>>;
+  EXPECT_LT(std::abs(std::sqrt(100 / static_cast<double>(1)) -
+                     rt100::num / static_cast<double>(rt100::den)),
+            5e-9);
+
+  using rt1000 = ratio_sqrt<std::ratio<1000>>;
+  EXPECT_LT(std::abs(std::sqrt(1000 / static_cast<double>(1)) -
+                     rt1000::num / static_cast<double>(rt1000::den)),
+            5e-9);
+
+  using rt10000 = ratio_sqrt<std::ratio<10000>>;
+  EXPECT_LT(std::abs(std::sqrt(10000 / static_cast<double>(1)) -
+                     rt10000::num / static_cast<double>(rt10000::den)),
+            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_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);
+}
+
+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));
+}
+
+TEST_F(TypeTraits, unit_t_traits) {
+  EXPECT_TRUE(
+      (std::is_same<void,
+                    traits::unit_t_traits<double>::underlying_type>::value));
+  EXPECT_TRUE(
+      (std::is_same<UNIT_LIB_DEFAULT_TYPE,
+                    traits::unit_t_traits<meter_t>::underlying_type>::value));
+  EXPECT_TRUE(
+      (std::is_same<void, traits::unit_t_traits<double>::value_type>::value));
+  EXPECT_TRUE(
+      (std::is_same<UNIT_LIB_DEFAULT_TYPE,
+                    traits::unit_t_traits<meter_t>::value_type>::value));
+}
+
+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));
+}
+
+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<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_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));
+}
+
+TEST_F(TypeTraits, inverse) {
+  double test;
+
+  using htz = inverse<seconds>;
+  bool shouldBeTrue = std::is_same<htz, hertz>::value;
+  EXPECT_TRUE(shouldBeTrue);
+
+  test = convert<inverse<celsius>, inverse<fahrenheit>>(1.0);
+  EXPECT_NEAR(5.0 / 9.0, test, 5.0e-5);
+
+  test = convert<inverse<kelvin>, inverse<fahrenheit>>(6.0);
+  EXPECT_NEAR(10.0 / 3.0, test, 5.0e-5);
+}
+
+TEST_F(TypeTraits, base_unit_of) {
+  using base = traits::base_unit_of<years>;
+  bool shouldBeTrue = std::is_same<base, category::time_unit>::value;
+
+  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));
+}
+
+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_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));
+}
+
+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));
+}
+
+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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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<substance::mole_t, substance::mole_t>::value));
+  EXPECT_FALSE((traits::is_substance_unit<year_t, substance::mole_t>::value));
+}
+
+TEST_F(TypeTraits, is_luminous_intensity_unit) {
+  EXPECT_TRUE((traits::is_luminous_intensity_unit<candela>::value));
+  EXPECT_FALSE(
+      (traits::is_luminous_intensity_unit<units::radiation::rad>::value));
+  EXPECT_FALSE((traits::is_luminous_intensity_unit<double>::value));
+
+  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<candela_t, candela_t>::value));
+  EXPECT_FALSE((traits::is_luminous_intensity_unit<rad_t, candela_t>::value));
+}
+
+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<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<degree_squared_t, steradian_t>::value));
+  EXPECT_FALSE(
+      (traits::is_solid_angle_unit<angle::degree_t, steradian_t>::value));
+}
+
+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<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));
+}
+
+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<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<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));
+}
+
+TEST_F(TypeTraits, is_acceleration_unit) {
+  EXPECT_TRUE((traits::is_acceleration_unit<meters_per_second_squared>::value));
+  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));
+
+  EXPECT_TRUE(
+      (traits::is_acceleration_unit<meters_per_second_squared_t>::value));
+  EXPECT_TRUE(
+      (traits::is_acceleration_unit<const meters_per_second_squared_t>::value));
+  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));
+  EXPECT_TRUE(
+      (traits::is_acceleration_unit<standard_gravity_t,
+                                    meters_per_second_squared_t>::value));
+  EXPECT_FALSE(
+      (traits::is_acceleration_unit<inch_t,
+                                    meters_per_second_squared_t>::value));
+}
+
+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<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));
+}
+
+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<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<atmosphere_t, pressure::pascal_t>::value));
+  EXPECT_FALSE((traits::is_pressure_unit<year_t, pressure::pascal_t>::value));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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<const farad_t&, millifarad_t>::value));
+  EXPECT_FALSE((traits::is_capacitance_unit<ohm_t, farad_t>::value));
+}
+
+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<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));
+}
+
+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<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<const siemens_t&, millisiemens_t>::value));
+  EXPECT_FALSE((traits::is_conductance_unit<volt_t, siemens_t>::value));
+}
+
+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<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));
+}
+
+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<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<gauss_t, tesla_t>::value));
+  EXPECT_FALSE(
+      (traits::is_magnetic_field_strength_unit<volt_t, tesla_t>::value));
+}
+
+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<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<const henry_t&, millihenry_t>::value));
+  EXPECT_FALSE((traits::is_inductance_unit<farad_t, henry_t>::value));
+}
+
+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<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<const lumen_t&, millilumen_t>::value));
+  EXPECT_FALSE((traits::is_luminous_flux_unit<pound_t, lumen_t>::value));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<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));
+}
+
+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<kilograms_per_cubic_meter_t>::value));
+  EXPECT_TRUE(
+      (traits::is_density_unit<const kilograms_per_cubic_meter_t>::value));
+  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));
+  EXPECT_FALSE(
+      (traits::is_density_unit<year_t, kilograms_per_cubic_meter_t>::value));
+}
+
+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<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));
+}
+
+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<gigabits_per_second_t>::value));
+  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));
+  EXPECT_TRUE(
+      (traits::is_data_transfer_rate_unit<gigabytes_per_second_t>::value));
+  EXPECT_FALSE((traits::is_data_transfer_rate_unit<year_t>::value));
+  EXPECT_TRUE(
+      (traits::is_data_transfer_rate_unit<gigabits_per_second_t,
+                                          gigabytes_per_second_t>::value));
+  EXPECT_FALSE(
+      (traits::is_data_transfer_rate_unit<year_t,
+                                          gigabytes_per_second_t>::value));
+}
+
+TEST_F(UnitManipulators, squared) {
+  double test;
+
+  test = convert<squared<meters>, square_feet>(0.092903);
+  EXPECT_NEAR(0.99999956944, test, 5.0e-12);
+
+  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;
+  EXPECT_TRUE(isSame);
+}
+
+TEST_F(UnitManipulators, cubed) {
+  double test;
+
+  test = convert<cubed<meters>, cubic_feet>(0.0283168);
+  EXPECT_NEAR(0.999998354619, test, 5.0e-13);
+}
+
+TEST_F(UnitManipulators, square_root) {
+  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_NEAR(1000.0, test, 5.0e-13);
+}
+
+TEST_F(UnitManipulators, compound_unit) {
+  using acceleration1 = unit<std::ratio<1>, category::acceleration_unit>;
+  using acceleration2 =
+      compound_unit<meters, inverse<seconds>, inverse<seconds>>;
+  using acceleration3 =
+      unit<std::ratio<1>,
+           base_unit<std::ratio<1>, std::ratio<0>, std::ratio<-2>>>;
+  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;
+
+  EXPECT_TRUE(areSame12);
+  EXPECT_TRUE(areSame23);
+  EXPECT_TRUE(areSame34);
+  EXPECT_TRUE(areSame45);
+
+  // test that thing with translations still compile
+  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));
+}
+
+TEST_F(UnitManipulators, dimensionalAnalysis) {
+  // these look like 'compound units', but the dimensional analysis can be
+  // REALLY handy if the unit types aren't know (i.e. they themselves are
+  // 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;
+  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;
+  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<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));
+}
+#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));
+}
+
+TEST_F(UnitContainer, make_unit) {
+  auto dist = units::make_unit<meter_t>(5);
+  EXPECT_EQ(meter_t(5), dist);
+}
+
+TEST_F(UnitContainer, unitTypeAddition) {
+  // units
+  meter_t a_m(1.0), c_m;
+  foot_t b_ft(3.28084);
+
+  double d = convert<feet, meters>(b_ft());
+  EXPECT_NEAR(1.0, d, 5.0e-5);
+
+  c_m = a_m + b_ft;
+  EXPECT_NEAR(2.0, c_m(), 5.0e-5);
+
+  c_m = b_ft + meter_t(3);
+  EXPECT_NEAR(4.0, c_m(), 5.0e-5);
+
+  auto e_ft = b_ft + meter_t(3);
+  EXPECT_NEAR(13.12336, e_ft(), 5.0e-6);
+
+  // scalar
+  scalar_t sresult = scalar_t(1.0) + scalar_t(1.0);
+  EXPECT_NEAR(2.0, sresult, 5.0e-6);
+
+  sresult = scalar_t(1.0) + 1.0;
+  EXPECT_NEAR(2.0, sresult, 5.0e-6);
+
+  sresult = 1.0 + scalar_t(1.0);
+  EXPECT_NEAR(2.0, sresult, 5.0e-6);
+
+  d = scalar_t(1.0) + scalar_t(1.0);
+  EXPECT_NEAR(2.0, d, 5.0e-6);
+
+  d = scalar_t(1.0) + 1.0;
+  EXPECT_NEAR(2.0, d, 5.0e-6);
+
+  d = 1.0 + scalar_t(1.0);
+  EXPECT_NEAR(2.0, d, 5.0e-6);
+}
+
+TEST_F(UnitContainer, unitTypeUnaryAddition) {
+  meter_t a_m(1.0);
+
+  EXPECT_EQ(++a_m, meter_t(2));
+  EXPECT_EQ(a_m++, meter_t(2));
+  EXPECT_EQ(a_m, meter_t(3));
+  EXPECT_EQ(+a_m, meter_t(3));
+  EXPECT_EQ(a_m, meter_t(3));
+
+  dBW_t b_dBW(1.0);
+
+  EXPECT_EQ(++b_dBW, dBW_t(2));
+  EXPECT_EQ(b_dBW++, dBW_t(2));
+  EXPECT_EQ(b_dBW, dBW_t(3));
+  EXPECT_EQ(+b_dBW, dBW_t(3));
+  EXPECT_EQ(b_dBW, dBW_t(3));
+}
+
+TEST_F(UnitContainer, unitTypeSubtraction) {
+  meter_t a_m(1.0), c_m;
+  foot_t b_ft(3.28084);
+
+  c_m = a_m - b_ft;
+  EXPECT_NEAR(0.0, c_m(), 5.0e-5);
+
+  c_m = b_ft - meter_t(1);
+  EXPECT_NEAR(0.0, c_m(), 5.0e-5);
+
+  auto e_ft = b_ft - meter_t(1);
+  EXPECT_NEAR(0.0, e_ft(), 5.0e-6);
+
+  scalar_t sresult = scalar_t(1.0) - scalar_t(1.0);
+  EXPECT_NEAR(0.0, sresult, 5.0e-6);
+
+  sresult = scalar_t(1.0) - 1.0;
+  EXPECT_NEAR(0.0, sresult, 5.0e-6);
+
+  sresult = 1.0 - scalar_t(1.0);
+  EXPECT_NEAR(0.0, sresult, 5.0e-6);
+
+  double d = scalar_t(1.0) - scalar_t(1.0);
+  EXPECT_NEAR(0.0, d, 5.0e-6);
+
+  d = scalar_t(1.0) - 1.0;
+  EXPECT_NEAR(0.0, d, 5.0e-6);
+
+  d = 1.0 - scalar_t(1.0);
+  EXPECT_NEAR(0.0, d, 5.0e-6);
+}
+
+TEST_F(UnitContainer, unitTypeUnarySubtraction) {
+  meter_t a_m(4.0);
+
+  EXPECT_EQ(--a_m, meter_t(3));
+  EXPECT_EQ(a_m--, meter_t(3));
+  EXPECT_EQ(a_m, meter_t(2));
+  EXPECT_EQ(-a_m, meter_t(-2));
+  EXPECT_EQ(a_m, meter_t(2));
+
+  dBW_t b_dBW(4.0);
+
+  EXPECT_EQ(--b_dBW, dBW_t(3));
+  EXPECT_EQ(b_dBW--, dBW_t(3));
+  EXPECT_EQ(b_dBW, dBW_t(2));
+  EXPECT_EQ(-b_dBW, dBW_t(-2));
+  EXPECT_EQ(b_dBW, dBW_t(2));
+}
+
+TEST_F(UnitContainer, unitTypeMultiplication) {
+  meter_t a_m(1.0), b_m(2.0);
+  foot_t a_ft(3.28084);
+
+  auto c_m2 = a_m * b_m;
+  EXPECT_NEAR(2.0, c_m2(), 5.0e-5);
+
+  c_m2 = b_m * meter_t(2);
+  EXPECT_NEAR(4.0, c_m2(), 5.0e-5);
+
+  c_m2 = b_m * a_ft;
+  EXPECT_NEAR(2.0, c_m2(), 5.0e-5);
+
+  auto c_m = b_m * 2.0;
+  EXPECT_NEAR(4.0, c_m(), 5.0e-5);
+
+  c_m = 2.0 * b_m;
+  EXPECT_NEAR(4.0, c_m(), 5.0e-5);
+
+  double convert = scalar_t(3.14);
+  EXPECT_NEAR(3.14, convert, 5.0e-5);
+
+  scalar_t sresult = scalar_t(5.0) * scalar_t(4.0);
+  EXPECT_NEAR(20.0, sresult(), 5.0e-5);
+
+  sresult = scalar_t(5.0) * 4.0;
+  EXPECT_NEAR(20.0, sresult(), 5.0e-5);
+
+  sresult = 4.0 * scalar_t(5.0);
+  EXPECT_NEAR(20.0, sresult(), 5.0e-5);
+
+  double result = scalar_t(5.0) * scalar_t(4.0);
+  EXPECT_NEAR(20.0, result, 5.0e-5);
+
+  result = scalar_t(5.0) * 4.0;
+  EXPECT_NEAR(20.0, result, 5.0e-5);
+
+  result = 4.0 * scalar_t(5.0);
+  EXPECT_NEAR(20.0, result, 5.0e-5);
+}
+
+TEST_F(UnitContainer, unitTypeMixedUnitMultiplication) {
+  meter_t a_m(1.0);
+  foot_t b_ft(3.28084);
+  unit_t<inverse<meter>> i_m(2.0);
+
+  // resultant unit is square of leftmost unit
+  auto c_m2 = a_m * b_ft;
+  EXPECT_NEAR(1.0, c_m2(), 5.0e-5);
+
+  auto c_ft2 = b_ft * a_m;
+  EXPECT_NEAR(10.7639111056, c_ft2(), 5.0e-7);
+
+  // you can get whatever (compatible) type you want if you ask explicitly
+  square_meter_t d_m2 = b_ft * a_m;
+  EXPECT_NEAR(1.0, d_m2(), 5.0e-5);
+
+  // a unit times a sclar ends up with the same units.
+  meter_t e_m = a_m * scalar_t(3.0);
+  EXPECT_NEAR(3.0, e_m(), 5.0e-5);
+
+  e_m = scalar_t(4.0) * a_m;
+  EXPECT_NEAR(4.0, e_m(), 5.0e-5);
+
+  // unit times its inverse results in a scalar
+  scalar_t s = a_m * i_m;
+  EXPECT_NEAR(2.0, s, 5.0e-5);
+
+  c_m2 = b_ft * meter_t(2);
+  EXPECT_NEAR(2.0, c_m2(), 5.0e-5);
+
+  auto e_ft2 = b_ft * meter_t(3);
+  EXPECT_NEAR(32.2917333168, e_ft2(), 5.0e-6);
+
+  auto mps = meter_t(10.0) * unit_t<inverse<seconds>>(1.0);
+  EXPECT_EQ(mps, meters_per_second_t(10));
+}
+
+TEST_F(UnitContainer, unitTypeScalarMultiplication) {
+  meter_t a_m(1.0);
+
+  auto result_m = scalar_t(3.0) * a_m;
+  EXPECT_NEAR(3.0, result_m(), 5.0e-5);
+
+  result_m = a_m * scalar_t(4.0);
+  EXPECT_NEAR(4.0, result_m(), 5.0e-5);
+
+  result_m = 3.0 * a_m;
+  EXPECT_NEAR(3.0, result_m(), 5.0e-5);
+
+  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;
+  EXPECT_TRUE(isSame);
+}
+
+TEST_F(UnitContainer, unitTypeDivision) {
+  meter_t a_m(1.0), b_m(2.0);
+  foot_t a_ft(3.28084);
+  second_t a_sec(10.0);
+  bool isSame;
+
+  auto c = a_m / a_ft;
+  EXPECT_NEAR(1.0, c, 5.0e-5);
+  isSame = std::is_same<decltype(c), scalar_t>::value;
+  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;
+  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;
+  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;
+  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;
+  EXPECT_TRUE(isSame);
+
+  double d = scalar_t(1.0) / 2.0;
+  EXPECT_NEAR(0.5, d, 5.0e-5);
+
+  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;
+  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;
+  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;
+  EXPECT_TRUE(isSame);
+
+  auto mph = mile_t(60.0) / hour_t(1.0);
+  meters_per_second_t mps = mph;
+  EXPECT_NEAR(26.8224, mps(), 5.0e-5);
+}
+
+TEST_F(UnitContainer, compoundAssignmentAddition) {
+  // units
+  meter_t a(0.0);
+  a += meter_t(1.0);
+
+  EXPECT_EQ(meter_t(1.0), a);
+
+  a += foot_t(meter_t(1));
+
+  EXPECT_EQ(meter_t(2.0), a);
+
+  // scalars
+  scalar_t b(0);
+  b += scalar_t(1.0);
+
+  EXPECT_EQ(scalar_t(1.0), b);
+
+  b += 1;
+
+  EXPECT_EQ(scalar_t(2.0), b);
+}
+
+TEST_F(UnitContainer, compoundAssignmentSubtraction) {
+  // units
+  meter_t a(2.0);
+  a -= meter_t(1.0);
+
+  EXPECT_EQ(meter_t(1.0), a);
+
+  a -= foot_t(meter_t(1));
+
+  EXPECT_EQ(meter_t(0.0), a);
+
+  // scalars
+  scalar_t b(2);
+  b -= scalar_t(1.0);
+
+  EXPECT_EQ(scalar_t(1.0), b);
+
+  b -= 1;
+
+  EXPECT_EQ(scalar_t(0), b);
+}
+
+TEST_F(UnitContainer, compoundAssignmentMultiplication) {
+  // units
+  meter_t a(2.0);
+  a *= scalar_t(2.0);
+
+  EXPECT_EQ(meter_t(4.0), a);
+
+  a *= 2.0;
+
+  EXPECT_EQ(meter_t(8.0), a);
+
+  // scalars
+  scalar_t b(2);
+  b *= scalar_t(2.0);
+
+  EXPECT_EQ(scalar_t(4.0), b);
+
+  b *= 2;
+
+  EXPECT_EQ(scalar_t(8.0), b);
+}
+
+TEST_F(UnitContainer, compoundAssignmentDivision) {
+  // units
+  meter_t a(8.0);
+  a /= scalar_t(2.0);
+
+  EXPECT_EQ(meter_t(4.0), a);
+
+  a /= 2.0;
+
+  EXPECT_EQ(meter_t(2.0), a);
+
+  // scalars
+  scalar_t b(8);
+  b /= scalar_t(2.0);
+
+  EXPECT_EQ(scalar_t(4.0), b);
+
+  b /= 2;
+
+  EXPECT_EQ(scalar_t(2.0), b);
+}
+
+TEST_F(UnitContainer, scalarTypeImplicitConversion) {
+  double test = scalar_t(3.0);
+  EXPECT_DOUBLE_EQ(3.0, test);
+
+  scalar_t testS = 3.0;
+  EXPECT_DOUBLE_EQ(3.0, testS);
+
+  scalar_t test3(ppm_t(10));
+  EXPECT_DOUBLE_EQ(0.00001, test3);
+
+  scalar_t test4;
+  test4 = ppm_t(1);
+  EXPECT_DOUBLE_EQ(0.000001, test4);
+}
+
+TEST_F(UnitContainer, valueMethod) {
+  double test = meter_t(3.0).to<double>();
+  EXPECT_DOUBLE_EQ(3.0, test);
+
+  auto test2 = meter_t(4.0).value();
+  EXPECT_DOUBLE_EQ(4.0, test2);
+  EXPECT_TRUE((std::is_same<decltype(test2), double>::value));
+}
+
+TEST_F(UnitContainer, convertMethod) {
+  double test = meter_t(3.0).convert<feet>().to<double>();
+  EXPECT_NEAR(9.84252, test, 5.0e-6);
+}
+
+#ifndef UNIT_LIB_DISABLE_IOSTREAM
+TEST_F(UnitContainer, cout) {
+  testing::internal::CaptureStdout();
+  std::cout << degree_t(349.87);
+  std::string output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("349.87 deg", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << meter_t(1.0);
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("1 m", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << dB_t(31.0);
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("31 dB", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << volt_t(21.79);
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("21.79 V", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << dBW_t(12.0);
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("12 dBW", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << dBm_t(120.0);
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("120 dBm", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << miles_per_hour_t(72.1);
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("72.1 mph", output.c_str());
+
+  // undefined unit
+  testing::internal::CaptureStdout();
+  std::cout << units::math::cpow<4>(meter_t(2));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("16 m^4", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << units::math::cpow<3>(foot_t(2));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("8 cu_ft", output.c_str());
+
+  testing::internal::CaptureStdout();
+  std::cout << std::setprecision(9) << units::math::cpow<4>(foot_t(2));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("0.138095597 m^4", output.c_str());
+
+  // constants
+  testing::internal::CaptureStdout();
+  std::cout << std::setprecision(8) << constants::k_B;
+  output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+  EXPECT_STREQ("1.3806485e-023 m^2 kg s^-2 K^-1", output.c_str());
+#else
+  EXPECT_STREQ("1.3806485e-23 m^2 kg s^-2 K^-1", output.c_str());
+#endif
+
+  testing::internal::CaptureStdout();
+  std::cout << std::setprecision(9) << constants::mu_B;
+  output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+  EXPECT_STREQ("9.27400999e-024 m^2 A", output.c_str());
+#else
+  EXPECT_STREQ("9.27400999e-24 m^2 A", output.c_str());
+#endif
+
+  testing::internal::CaptureStdout();
+  std::cout << std::setprecision(7) << constants::sigma;
+  output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+  EXPECT_STREQ("5.670367e-008 kg s^-3 K^-4", output.c_str());
+#else
+  EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
+#endif
+}
+
+TEST_F(UnitContainer, to_string) {
+  foot_t a(3.5);
+  EXPECT_STREQ("3.5 ft", units::length::to_string(a).c_str());
+
+  meter_t b(8);
+  EXPECT_STREQ("8 m", units::length::to_string(b).c_str());
+}
+
+TEST_F(UnitContainer, DISABLED_to_string_locale) {
+  struct lconv* lc;
+
+  // German locale
+#if defined(_MSC_VER)
+  setlocale(LC_ALL, "de-DE");
+#else
+  EXPECT_STREQ("de_DE.utf8", setlocale(LC_ALL, "de_DE.utf8"));
+#endif
+
+  lc = localeconv();
+  char point_de = *lc->decimal_point;
+  EXPECT_EQ(point_de, ',');
+
+  kilometer_t de = 2_km;
+  EXPECT_STREQ("2 km", units::length::to_string(de).c_str());
+
+  de = 2.5_km;
+  EXPECT_STREQ("2,5 km", units::length::to_string(de).c_str());
+
+  // US locale
+#if defined(_MSC_VER)
+  setlocale(LC_ALL, "en-US");
+#else
+  EXPECT_STREQ("en_US.utf8", setlocale(LC_ALL, "en_US.utf8"));
+#endif
+
+  lc = localeconv();
+  char point_us = *lc->decimal_point;
+  EXPECT_EQ(point_us, '.');
+
+  mile_t us = 2_mi;
+  EXPECT_STREQ("2 mi", units::length::to_string(us).c_str());
+
+  us = 2.5_mi;
+  EXPECT_STREQ("2.5 mi", units::length::to_string(us).c_str());
+}
+
+TEST_F(UnitContainer, nameAndAbbreviation) {
+  foot_t a(3.5);
+  EXPECT_STREQ("ft", units::abbreviation(a));
+  EXPECT_STREQ("ft", a.abbreviation());
+  EXPECT_STREQ("foot", a.name());
+
+  meter_t b(8);
+  EXPECT_STREQ("m", units::abbreviation(b));
+  EXPECT_STREQ("m", b.abbreviation());
+  EXPECT_STREQ("meter", b.name());
+}
+#endif
+
+TEST_F(UnitContainer, negative) {
+  meter_t a(5.3);
+  meter_t b(-5.3);
+  EXPECT_NEAR(a.to<double>(), -b.to<double>(), 5.0e-320);
+  EXPECT_NEAR(b.to<double>(), -a.to<double>(), 5.0e-320);
+
+  dB_t c(2.87);
+  dB_t d(-2.87);
+  EXPECT_NEAR(c.to<double>(), -d.to<double>(), 5.0e-320);
+  EXPECT_NEAR(d.to<double>(), -c.to<double>(), 5.0e-320);
+
+  ppm_t e = -1 * ppm_t(10);
+  EXPECT_EQ(e, -ppm_t(10));
+  EXPECT_NEAR(-0.00001, e, 5.0e-10);
+}
+
+TEST_F(UnitContainer, concentration) {
+  ppb_t a(ppm_t(1));
+  EXPECT_EQ(ppb_t(1000), a);
+  EXPECT_EQ(0.000001, a);
+  EXPECT_EQ(0.000001, a.to<double>());
+
+  scalar_t b(ppm_t(1));
+  EXPECT_EQ(0.000001, b);
+
+  scalar_t c = ppb_t(1);
+  EXPECT_EQ(0.000000001, c);
+}
+
+TEST_F(UnitContainer, dBConversion) {
+  dBW_t a_dbw(23.1);
+  watt_t a_w = a_dbw;
+  dBm_t a_dbm = a_dbw;
+
+  EXPECT_NEAR(204.173794, a_w(), 5.0e-7);
+  EXPECT_NEAR(53.1, a_dbm(), 5.0e-7);
+
+  milliwatt_t b_mw(100000.0);
+  watt_t b_w = b_mw;
+  dBm_t b_dbm = b_mw;
+  dBW_t b_dbw = b_mw;
+
+  EXPECT_NEAR(100.0, b_w(), 5.0e-7);
+  EXPECT_NEAR(50.0, b_dbm(), 5.0e-7);
+  EXPECT_NEAR(20.0, b_dbw(), 5.0e-7);
+}
+
+TEST_F(UnitContainer, dBAddition) {
+  bool isSame;
+
+  auto result_dbw = dBW_t(10.0) + dB_t(30.0);
+  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;
+  EXPECT_TRUE(isSame);
+
+  auto result_dbm = dB_t(30.0) + dBm_t(20.0);
+  EXPECT_NEAR(50.0, result_dbm(), 5.0e-5);
+
+  // adding dBW to dBW is something you probably shouldn't do, but let's see if
+  // 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;
+  EXPECT_TRUE(isSame);
+}
+
+TEST_F(UnitContainer, dBSubtraction) {
+  bool isSame;
+
+  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;
+  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;
+  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;
+  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;
+  EXPECT_TRUE(isSame);
+}
+
+TEST_F(UnitContainer, unit_cast) {
+  meter_t test1(5.7);
+  hectare_t test2(16);
+
+  double dResult1 = 5.7;
+
+  double dResult2 = 16;
+  int iResult2 = 16;
+
+  EXPECT_EQ(dResult1, unit_cast<double>(test1));
+  EXPECT_EQ(dResult2, unit_cast<double>(test2));
+  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));
+}
+
+// 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(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(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(meter_t(10) == x);
+
+  // conversion using literal syntax
+  foot_t y = 0.3048_m;
+  EXPECT_TRUE(1_ft == y);
+
+  // Pythagorean theorem
+  meter_t a = 3_m;
+  meter_t b = 4_m;
+  meter_t c = sqrt(pow<2>(a) + pow<2>(b));
+  EXPECT_TRUE(c == 5_m);
+}
+#endif
+
+TEST_F(UnitConversion, length) {
+  double test;
+  test = convert<meters, nanometers>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, micrometers>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, millimeters>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, centimeters>(0.01);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, kilometers>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, meters>(1.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, feet>(0.3048);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, miles>(1609.344);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, inches>(0.0254);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, nauticalMiles>(1852.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, astronicalUnits>(149597870700.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, lightyears>(9460730472580800.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<meters, parsec>(3.08567758e16);
+  EXPECT_NEAR(1.0, test, 5.0e7);
+
+  test = convert<feet, feet>(6.3);
+  EXPECT_NEAR(6.3, test, 5.0e-5);
+  test = convert<feet, inches>(6.0);
+  EXPECT_NEAR(72.0, test, 5.0e-5);
+  test = convert<inches, feet>(6.0);
+  EXPECT_NEAR(0.5, test, 5.0e-5);
+  test = convert<meter, feet>(1.0);
+  EXPECT_NEAR(3.28084, test, 5.0e-5);
+  test = convert<miles, nauticalMiles>(6.3);
+  EXPECT_NEAR(5.47455, test, 5.0e-6);
+  test = convert<miles, meters>(11.0);
+  EXPECT_NEAR(17702.8, test, 5.0e-2);
+  test = convert<meters, chains>(1.0);
+  EXPECT_NEAR(0.0497097, test, 5.0e-7);
+}
+
+TEST_F(UnitConversion, mass) {
+  double test;
+
+  test = convert<kilograms, grams>(1.0e-3);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, micrograms>(1.0e-9);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, milligrams>(1.0e-6);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, kilograms>(1.0);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, metric_tons>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, pounds>(0.453592);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, long_tons>(1016.05);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, short_tons>(907.185);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, mass::ounces>(0.0283495);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<kilograms, carats>(0.0002);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<slugs, kilograms>(1.0);
+  EXPECT_NEAR(14.593903, test, 5.0e-7);
+
+  test = convert<pounds, carats>(6.3);
+  EXPECT_NEAR(14288.2, test, 5.0e-2);
+}
+
+TEST_F(UnitConversion, time) {
+  double result = 0;
+  double daysPerYear = 365;
+  double hoursPerDay = 24;
+  double minsPerHour = 60;
+  double secsPerMin = 60;
+  double daysPerWeek = 7;
+
+  result = 2 * daysPerYear * hoursPerDay * minsPerHour * secsPerMin *
+           (1 / minsPerHour) * (1 / secsPerMin) * (1 / hoursPerDay) *
+           (1 / daysPerWeek);
+  EXPECT_NEAR(104.286, result, 5.0e-4);
+
+  year_t twoYears(2.0);
+  week_t twoYearsInWeeks = twoYears;
+  EXPECT_NEAR(week_t(104.286).to<double>(), twoYearsInWeeks.to<double>(),
+              5.0e-4);
+
+  double test;
+
+  test = convert<seconds, seconds>(1.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, nanoseconds>(1.0e-9);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, microseconds>(1.0e-6);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, milliseconds>(1.0e-3);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, minutes>(60.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, hours>(3600.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, days>(86400.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, weeks>(604800.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<seconds, years>(3.154e7);
+  EXPECT_NEAR(1.0, test, 5.0e3);
+
+  test = convert<years, weeks>(2.0);
+  EXPECT_NEAR(104.2857142857143, test, 5.0e-14);
+  test = convert<hours, minutes>(4.0);
+  EXPECT_NEAR(240.0, test, 5.0e-14);
+  test = convert<julian_years, days>(1.0);
+  EXPECT_NEAR(365.25, test, 5.0e-14);
+  test = convert<gregorian_years, days>(1.0);
+  EXPECT_NEAR(365.2425, test, 5.0e-14);
+}
+
+TEST_F(UnitConversion, angle) {
+  angle::degree_t quarterCircleDeg(90.0);
+  angle::radian_t quarterCircleRad = quarterCircleDeg;
+  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).to<double>(),
+              quarterCircleRad.to<double>(), 5.0e-12);
+
+  double test;
+
+  test = convert<angle::radians, angle::radians>(1.0);
+  EXPECT_NEAR(1.0, test, 5.0e-20);
+  test = convert<angle::radians, angle::milliradians>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-4);
+  test = convert<angle::radians, angle::degrees>(0.0174533);
+  EXPECT_NEAR(1.0, test, 5.0e-7);
+  test = convert<angle::radians, angle::arcminutes>(0.000290888);
+  EXPECT_NEAR(0.99999928265913, test, 5.0e-8);
+  test = convert<angle::radians, angle::arcseconds>(4.8481e-6);
+  EXPECT_NEAR(0.999992407, test, 5.0e-10);
+  test = convert<angle::radians, angle::turns>(6.28319);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+  test = convert<angle::radians, angle::gradians>(0.015708);
+  EXPECT_NEAR(1.0, test, 5.0e-6);
+
+  test = convert<angle::radians, angle::radians>(2.1);
+  EXPECT_NEAR(2.1, test, 5.0e-6);
+  test = convert<angle::arcseconds, angle::gradians>(2.1);
+  EXPECT_NEAR(0.000648148, test, 5.0e-6);
+  test = convert<angle::radians, angle::degrees>(constants::detail::PI_VAL);
+  EXPECT_NEAR(180.0, test, 5.0e-6);
+  test = convert<angle::degrees, angle::radians>(90.0);
+  EXPECT_NEAR(constants::detail::PI_VAL / 2, test, 5.0e-6);
+}
+
+TEST_F(UnitConversion, current) {
+  double test;
+
+  test = convert<current::A, current::mA>(2.1);
+  EXPECT_NEAR(2100.0, test, 5.0e-6);
+}
+
+TEST_F(UnitConversion, temperature) {
+  // temp conversion are weird/hard since they involve translations AND scaling.
+  double test;
+
+  test = convert<kelvin, kelvin>(72.0);
+  EXPECT_NEAR(72.0, test, 5.0e-5);
+  test = convert<fahrenheit, fahrenheit>(72.0);
+  EXPECT_NEAR(72.0, test, 5.0e-5);
+  test = convert<kelvin, fahrenheit>(300.0);
+  EXPECT_NEAR(80.33, test, 5.0e-5);
+  test = convert<fahrenheit, kelvin>(451.0);
+  EXPECT_NEAR(505.928, test, 5.0e-4);
+  test = convert<kelvin, celsius>(300.0);
+  EXPECT_NEAR(26.85, test, 5.0e-3);
+  test = convert<celsius, kelvin>(451.0);
+  EXPECT_NEAR(724.15, test, 5.0e-3);
+  test = convert<fahrenheit, celsius>(72.0);
+  EXPECT_NEAR(22.2222, test, 5.0e-5);
+  test = convert<celsius, fahrenheit>(100.0);
+  EXPECT_NEAR(212.0, test, 5.0e-5);
+  test = convert<fahrenheit, celsius>(32.0);
+  EXPECT_NEAR(0.0, test, 5.0e-5);
+  test = convert<celsius, fahrenheit>(0.0);
+  EXPECT_NEAR(32.0, test, 5.0e-5);
+  test = convert<rankine, kelvin>(100.0);
+  EXPECT_NEAR(55.5556, test, 5.0e-5);
+  test = convert<kelvin, rankine>(100.0);
+  EXPECT_NEAR(180.0, test, 5.0e-5);
+  test = convert<fahrenheit, rankine>(100.0);
+  EXPECT_NEAR(559.67, test, 5.0e-5);
+  test = convert<rankine, fahrenheit>(72.0);
+  EXPECT_NEAR(-387.67, test, 5.0e-5);
+  test = convert<reaumur, kelvin>(100.0);
+  EXPECT_NEAR(398.0, test, 5.0e-1);
+  test = convert<reaumur, celsius>(80.0);
+  EXPECT_NEAR(100.0, test, 5.0e-5);
+  test = convert<celsius, reaumur>(212.0);
+  EXPECT_NEAR(169.6, test, 5.0e-2);
+  test = convert<reaumur, fahrenheit>(80.0);
+  EXPECT_NEAR(212.0, test, 5.0e-5);
+  test = convert<fahrenheit, reaumur>(37.0);
+  EXPECT_NEAR(2.222, test, 5.0e-3);
+}
+
+TEST_F(UnitConversion, luminous_intensity) {
+  double test;
+
+  test = convert<candela, millicandela>(72.0);
+  EXPECT_NEAR(72000.0, test, 5.0e-5);
+  test = convert<millicandela, candela>(376.0);
+  EXPECT_NEAR(0.376, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, solid_angle) {
+  double test;
+  bool same;
+
+  same = std::is_same<traits::base_unit_of<steradians>,
+                      traits::base_unit_of<degrees_squared>>::value;
+  EXPECT_TRUE(same);
+
+  test = convert<steradians, steradians>(72.0);
+  EXPECT_NEAR(72.0, test, 5.0e-5);
+  test = convert<steradians, degrees_squared>(1.0);
+  EXPECT_NEAR(3282.8, test, 5.0e-2);
+  test = convert<steradians, spats>(8.0);
+  EXPECT_NEAR(0.636619772367582, test, 5.0e-14);
+  test = convert<degrees_squared, steradians>(3282.8);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<degrees_squared, degrees_squared>(72.0);
+  EXPECT_NEAR(72.0, test, 5.0e-5);
+  test = convert<degrees_squared, spats>(3282.8);
+  EXPECT_NEAR(1.0 / (4 * constants::detail::PI_VAL), test, 5.0e-5);
+  test = convert<spats, steradians>(1.0 / (4 * constants::detail::PI_VAL));
+  EXPECT_NEAR(1.0, test, 5.0e-14);
+  test = convert<spats, degrees_squared>(1.0 / (4 * constants::detail::PI_VAL));
+  EXPECT_NEAR(3282.8, test, 5.0e-2);
+  test = convert<spats, spats>(72.0);
+  EXPECT_NEAR(72.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, frequency) {
+  double test;
+
+  test = convert<hertz, kilohertz>(63000.0);
+  EXPECT_NEAR(63.0, test, 5.0e-5);
+  test = convert<hertz, hertz>(6.3);
+  EXPECT_NEAR(6.3, test, 5.0e-5);
+  test = convert<kilohertz, hertz>(5.0);
+  EXPECT_NEAR(5000.0, test, 5.0e-5);
+  test = convert<megahertz, hertz>(1.0);
+  EXPECT_NEAR(1.0e6, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, velocity) {
+  double test;
+  bool same;
+
+  same = std::is_same<meters_per_second,
+                      unit<std::ratio<1>, category::velocity_unit>>::value;
+  EXPECT_TRUE(same);
+  same = traits::is_convertible_unit<miles_per_hour, meters_per_second>::value;
+  EXPECT_TRUE(same);
+
+  test = convert<meters_per_second, miles_per_hour>(1250.0);
+  EXPECT_NEAR(2796.17, test, 5.0e-3);
+  test = convert<feet_per_second, kilometers_per_hour>(2796.17);
+  EXPECT_NEAR(3068.181418, test, 5.0e-7);
+  test = convert<knots, miles_per_hour>(600.0);
+  EXPECT_NEAR(690.468, test, 5.0e-4);
+  test = convert<miles_per_hour, feet_per_second>(120.0);
+  EXPECT_NEAR(176.0, test, 5.0e-5);
+  test = convert<feet_per_second, meters_per_second>(10.0);
+  EXPECT_NEAR(3.048, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, angular_velocity) {
+  double test;
+  bool same;
+
+  same =
+      std::is_same<radians_per_second,
+                   unit<std::ratio<1>, category::angular_velocity_unit>>::value;
+  EXPECT_TRUE(same);
+  same = traits::is_convertible_unit<rpm, radians_per_second>::value;
+  EXPECT_TRUE(same);
+
+  test = convert<radians_per_second, milliarcseconds_per_year>(1.0);
+  EXPECT_NEAR(6.504e15, test, 1.0e12);
+  test = convert<degrees_per_second, radians_per_second>(1.0);
+  EXPECT_NEAR(0.0174533, test, 5.0e-8);
+  test = convert<rpm, radians_per_second>(1.0);
+  EXPECT_NEAR(0.10471975512, test, 5.0e-13);
+  test = convert<milliarcseconds_per_year, radians_per_second>(1.0);
+  EXPECT_NEAR(1.537e-16, test, 5.0e-20);
+}
+
+TEST_F(UnitConversion, acceleration) {
+  double test;
+
+  test = convert<standard_gravity, meters_per_second_squared>(1.0);
+  EXPECT_NEAR(9.80665, test, 5.0e-10);
+}
+
+TEST_F(UnitConversion, force) {
+  double test;
+
+  test = convert<units::force::newton, units::force::newton>(1.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<units::force::newton, units::force::pounds>(6.3);
+  EXPECT_NEAR(1.4163, test, 5.0e-5);
+  test = convert<units::force::newton, units::force::dynes>(5.0);
+  EXPECT_NEAR(500000.0, test, 5.0e-5);
+  test = convert<units::force::newtons, units::force::poundals>(2.1);
+  EXPECT_NEAR(15.1893, test, 5.0e-5);
+  test = convert<units::force::newtons, units::force::kiloponds>(173.0);
+  EXPECT_NEAR(17.6411, test, 5.0e-5);
+  test = convert<units::force::poundals, units::force::kiloponds>(21.879);
+  EXPECT_NEAR(0.308451933, test, 5.0e-10);
+}
+
+TEST_F(UnitConversion, area) {
+  double test;
+
+  test = convert<hectares, acres>(6.3);
+  EXPECT_NEAR(15.5676, test, 5.0e-5);
+  test = convert<square_miles, square_kilometers>(10.0);
+  EXPECT_NEAR(25.8999, test, 5.0e-5);
+  test = convert<square_inch, square_meter>(4.0);
+  EXPECT_NEAR(0.00258064, test, 5.0e-9);
+  test = convert<acre, square_foot>(5.0);
+  EXPECT_NEAR(217800.0, test, 5.0e-5);
+  test = convert<square_meter, square_foot>(1.0);
+  EXPECT_NEAR(10.7639, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, pressure) {
+  double test;
+
+  test = convert<pascals, torr>(1.0);
+  EXPECT_NEAR(0.00750062, test, 5.0e-5);
+  test = convert<bar, psi>(2.2);
+  EXPECT_NEAR(31.9083, test, 5.0e-5);
+  test = convert<atmospheres, bar>(4.0);
+  EXPECT_NEAR(4.053, test, 5.0e-5);
+  test = convert<torr, pascals>(800.0);
+  EXPECT_NEAR(106657.89474, test, 5.0e-5);
+  test = convert<psi, atmospheres>(38.0);
+  EXPECT_NEAR(2.58575, test, 5.0e-5);
+  test = convert<psi, pascals>(1.0);
+  EXPECT_NEAR(6894.76, test, 5.0e-3);
+  test = convert<pascals, bar>(0.25);
+  EXPECT_NEAR(2.5e-6, test, 5.0e-5);
+  test = convert<torr, atmospheres>(9.0);
+  EXPECT_NEAR(0.0118421, test, 5.0e-8);
+  test = convert<bar, torr>(12.0);
+  EXPECT_NEAR(9000.74, test, 5.0e-3);
+  test = convert<atmospheres, psi>(1.0);
+  EXPECT_NEAR(14.6959, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, charge) {
+  double test;
+
+  test = convert<coulombs, ampere_hours>(4.0);
+  EXPECT_NEAR(0.00111111, test, 5.0e-9);
+  test = convert<ampere_hours, coulombs>(1.0);
+  EXPECT_NEAR(3600.0, test, 5.0e-6);
+}
+
+TEST_F(UnitConversion, energy) {
+  double test;
+
+  test = convert<joules, calories>(8000.000464);
+  EXPECT_NEAR(1912.046, test, 5.0e-4);
+  test = convert<therms, joules>(12.0);
+  EXPECT_NEAR(1.266e+9, test, 5.0e5);
+  test = convert<megajoules, watt_hours>(100.0);
+  EXPECT_NEAR(27777.778, test, 5.0e-4);
+  test = convert<kilocalories, megajoules>(56.0);
+  EXPECT_NEAR(0.234304, test, 5.0e-7);
+  test = convert<kilojoules, therms>(56.0);
+  EXPECT_NEAR(0.000530904, test, 5.0e-5);
+  test = convert<british_thermal_units, kilojoules>(18.56399995447);
+  EXPECT_NEAR(19.5860568, test, 5.0e-5);
+  test = convert<calories, energy::foot_pounds>(18.56399995447);
+  EXPECT_NEAR(57.28776190423856, test, 5.0e-5);
+  test = convert<megajoules, calories>(1.0);
+  EXPECT_NEAR(239006.0, test, 5.0e-1);
+  test = convert<kilocalories, kilowatt_hours>(2.0);
+  EXPECT_NEAR(0.00232444, test, 5.0e-9);
+  test = convert<therms, kilocalories>(0.1);
+  EXPECT_NEAR(2521.04, test, 5.0e-3);
+  test = convert<watt_hours, megajoules>(67.0);
+  EXPECT_NEAR(0.2412, test, 5.0e-5);
+  test = convert<british_thermal_units, watt_hours>(100.0);
+  EXPECT_NEAR(29.3071, test, 5.0e-5);
+  test = convert<calories, BTU>(100.0);
+  EXPECT_NEAR(0.396567, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, power) {
+  double test;
+
+  test = convert<compound_unit<energy::foot_pounds, inverse<seconds>>, watts>(
+      550.0);
+  EXPECT_NEAR(745.7, test, 5.0e-2);
+  test = convert<watts, gigawatts>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-4);
+  test = convert<microwatts, watts>(200000.0);
+  EXPECT_NEAR(0.2, test, 5.0e-4);
+  test = convert<horsepower, watts>(100.0);
+  EXPECT_NEAR(74570.0, test, 5.0e-1);
+  test = convert<horsepower, megawatts>(5.0);
+  EXPECT_NEAR(0.0037284994, test, 5.0e-7);
+  test = convert<kilowatts, horsepower>(232.0);
+  EXPECT_NEAR(311.117, test, 5.0e-4);
+  test = convert<milliwatts, horsepower>(1001.0);
+  EXPECT_NEAR(0.001342363, test, 5.0e-9);
+}
+
+TEST_F(UnitConversion, voltage) {
+  double test;
+
+  test = convert<volts, millivolts>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picovolts, volts>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanovolts, volts>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microvolts, volts>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millivolts, volts>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilovolts, volts>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megavolts, volts>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigavolts, volts>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<statvolts, volts>(299.792458);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millivolts, statvolts>(1000.0);
+  EXPECT_NEAR(299.792458, test, 5.0e-5);
+  test = convert<abvolts, nanovolts>(0.1);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microvolts, abvolts>(0.01);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, capacitance) {
+  double test;
+
+  test = convert<farads, millifarads>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picofarads, farads>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanofarads, farads>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microfarads, farads>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millifarads, farads>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilofarads, farads>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megafarads, farads>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigafarads, farads>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, impedance) {
+  double test;
+
+  test = convert<ohms, milliohms>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picoohms, ohms>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanoohms, ohms>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microohms, ohms>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<milliohms, ohms>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kiloohms, ohms>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megaohms, ohms>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigaohms, ohms>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, conductance) {
+  double test;
+
+  test = convert<siemens, millisiemens>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picosiemens, siemens>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanosiemens, siemens>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microsiemens, siemens>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millisiemens, siemens>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilosiemens, siemens>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megasiemens, siemens>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigasiemens, siemens>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, magnetic_flux) {
+  double test;
+
+  test = convert<webers, milliwebers>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picowebers, webers>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanowebers, webers>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microwebers, webers>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<milliwebers, webers>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilowebers, webers>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megawebers, webers>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigawebers, webers>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<maxwells, webers>(100000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanowebers, maxwells>(10.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, magnetic_field_strength) {
+  double test;
+
+  test = convert<teslas, milliteslas>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picoteslas, teslas>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanoteslas, teslas>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microteslas, teslas>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<milliteslas, teslas>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kiloteslas, teslas>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megateslas, teslas>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigateslas, teslas>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gauss, teslas>(10000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanoteslas, gauss>(100000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, inductance) {
+  double test;
+
+  test = convert<henries, millihenries>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picohenries, henries>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanohenries, henries>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microhenries, henries>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millihenries, henries>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilohenries, henries>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megahenries, henries>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigahenries, henries>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, luminous_flux) {
+  double test;
+
+  test = convert<lumens, millilumens>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picolumens, lumens>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanolumens, lumens>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microlumens, lumens>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millilumens, lumens>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilolumens, lumens>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megalumens, lumens>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigalumens, lumens>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, illuminance) {
+  double test;
+
+  test = convert<luxes, milliluxes>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picoluxes, luxes>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanoluxes, luxes>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microluxes, luxes>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<milliluxes, luxes>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kiloluxes, luxes>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megaluxes, luxes>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigaluxes, luxes>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+
+  test = convert<footcandles, luxes>(0.092903);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<lux, lumens_per_square_inch>(1550.0031000062);
+  EXPECT_NEAR(1.0, test, 5.0e-13);
+  test = convert<phots, luxes>(0.0001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, radiation) {
+  double test;
+
+  test = convert<becquerels, millibecquerels>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picobecquerels, becquerels>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanobecquerels, becquerels>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microbecquerels, becquerels>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millibecquerels, becquerels>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilobecquerels, becquerels>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megabecquerels, becquerels>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigabecquerels, becquerels>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+
+  test = convert<grays, milligrays>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picograys, grays>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanograys, grays>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<micrograys, grays>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<milligrays, grays>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilograys, grays>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megagrays, grays>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigagrays, grays>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+
+  test = convert<sieverts, millisieverts>(10.0);
+  EXPECT_NEAR(10000.0, test, 5.0e-5);
+  test = convert<picosieverts, sieverts>(1000000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<nanosieverts, sieverts>(1000000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<microsieverts, sieverts>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<millisieverts, sieverts>(1000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<kilosieverts, sieverts>(0.001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<megasieverts, sieverts>(0.000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<gigasieverts, sieverts>(0.000000001);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+
+  test = convert<becquerels, curies>(37.0e9);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<becquerels, rutherfords>(1000000.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<rads, grays>(100.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, torque) {
+  double test;
+
+  test = convert<torque::foot_pounds, newton_meter>(1.0);
+  EXPECT_NEAR(1.355817948, test, 5.0e-5);
+  test = convert<inch_pounds, newton_meter>(1.0);
+  EXPECT_NEAR(0.112984829, test, 5.0e-5);
+  test = convert<foot_poundals, newton_meter>(1.0);
+  EXPECT_NEAR(4.214011009e-2, test, 5.0e-5);
+  test = convert<meter_kilograms, newton_meter>(1.0);
+  EXPECT_NEAR(9.80665, test, 5.0e-5);
+  test = convert<inch_pound, meter_kilogram>(86.79616930855788);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<foot_poundals, inch_pound>(2.681170713);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, volume) {
+  double test;
+
+  test = convert<cubic_meters, cubic_meter>(1.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<cubic_millimeters, cubic_meter>(1.0);
+  EXPECT_NEAR(1.0e-9, test, 5.0e-5);
+  test = convert<cubic_kilometers, cubic_meter>(1.0);
+  EXPECT_NEAR(1.0e9, test, 5.0e-5);
+  test = convert<liters, cubic_meter>(1.0);
+  EXPECT_NEAR(0.001, test, 5.0e-5);
+  test = convert<milliliters, cubic_meter>(1.0);
+  EXPECT_NEAR(1.0e-6, test, 5.0e-5);
+  test = convert<cubic_inches, cubic_meter>(1.0);
+  EXPECT_NEAR(1.6387e-5, test, 5.0e-10);
+  test = convert<cubic_feet, cubic_meter>(1.0);
+  EXPECT_NEAR(0.0283168, test, 5.0e-8);
+  test = convert<cubic_yards, cubic_meter>(1.0);
+  EXPECT_NEAR(0.764555, test, 5.0e-7);
+  test = convert<cubic_miles, cubic_meter>(1.0);
+  EXPECT_NEAR(4.168e+9, test, 5.0e5);
+  test = convert<gallons, cubic_meter>(1.0);
+  EXPECT_NEAR(0.00378541, test, 5.0e-8);
+  test = convert<quarts, cubic_meter>(1.0);
+  EXPECT_NEAR(0.000946353, test, 5.0e-10);
+  test = convert<pints, cubic_meter>(1.0);
+  EXPECT_NEAR(0.000473176, test, 5.0e-10);
+  test = convert<cups, cubic_meter>(1.0);
+  EXPECT_NEAR(0.00024, test, 5.0e-6);
+  test = convert<volume::fluid_ounces, cubic_meter>(1.0);
+  EXPECT_NEAR(2.9574e-5, test, 5.0e-5);
+  test = convert<barrels, cubic_meter>(1.0);
+  EXPECT_NEAR(0.158987294928, test, 5.0e-13);
+  test = convert<bushels, cubic_meter>(1.0);
+  EXPECT_NEAR(0.0352391, test, 5.0e-8);
+  test = convert<cords, cubic_meter>(1.0);
+  EXPECT_NEAR(3.62456, test, 5.0e-6);
+  test = convert<cubic_fathoms, cubic_meter>(1.0);
+  EXPECT_NEAR(6.11644, test, 5.0e-6);
+  test = convert<tablespoons, cubic_meter>(1.0);
+  EXPECT_NEAR(1.4787e-5, test, 5.0e-10);
+  test = convert<teaspoons, cubic_meter>(1.0);
+  EXPECT_NEAR(4.9289e-6, test, 5.0e-11);
+  test = convert<pinches, cubic_meter>(1.0);
+  EXPECT_NEAR(616.11519921875e-9, test, 5.0e-20);
+  test = convert<dashes, cubic_meter>(1.0);
+  EXPECT_NEAR(308.057599609375e-9, test, 5.0e-20);
+  test = convert<drops, cubic_meter>(1.0);
+  EXPECT_NEAR(82.14869322916e-9, test, 5.0e-9);
+  test = convert<fifths, cubic_meter>(1.0);
+  EXPECT_NEAR(0.00075708236, test, 5.0e-12);
+  test = convert<drams, cubic_meter>(1.0);
+  EXPECT_NEAR(3.69669e-6, test, 5.0e-12);
+  test = convert<gills, cubic_meter>(1.0);
+  EXPECT_NEAR(0.000118294, test, 5.0e-10);
+  test = convert<pecks, cubic_meter>(1.0);
+  EXPECT_NEAR(0.00880977, test, 5.0e-9);
+  test = convert<sacks, cubic_meter>(9.4591978);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<shots, cubic_meter>(1.0);
+  EXPECT_NEAR(4.43603e-5, test, 5.0e-11);
+  test = convert<strikes, cubic_meter>(1.0);
+  EXPECT_NEAR(0.07047814033376, test, 5.0e-5);
+  test = convert<volume::fluid_ounces, milliliters>(1.0);
+  EXPECT_NEAR(29.5735, test, 5.0e-5);
+}
+
+TEST_F(UnitConversion, density) {
+  double test;
+
+  test = convert<kilograms_per_cubic_meter, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(1.0, test, 5.0e-5);
+  test = convert<grams_per_milliliter, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(1000.0, test, 5.0e-5);
+  test = convert<kilograms_per_liter, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(1000.0, test, 5.0e-5);
+  test = convert<ounces_per_cubic_foot, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(1.001153961, test, 5.0e-10);
+  test = convert<ounces_per_cubic_inch, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(1.729994044e3, test, 5.0e-7);
+  test = convert<ounces_per_gallon, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(7.489151707, test, 5.0e-10);
+  test = convert<pounds_per_cubic_foot, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(16.01846337, test, 5.0e-9);
+  test = convert<pounds_per_cubic_inch, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(2.767990471e4, test, 5.0e-6);
+  test = convert<pounds_per_gallon, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(119.8264273, test, 5.0e-8);
+  test = convert<slugs_per_cubic_foot, kilograms_per_cubic_meter>(1.0);
+  EXPECT_NEAR(515.3788184, test, 5.0e-6);
+}
+
+TEST_F(UnitConversion, concentration) {
+  double test;
+
+  test = ppm_t(1.0);
+  EXPECT_NEAR(1.0e-6, test, 5.0e-12);
+  test = ppb_t(1.0);
+  EXPECT_NEAR(1.0e-9, test, 5.0e-12);
+  test = ppt_t(1.0);
+  EXPECT_NEAR(1.0e-12, test, 5.0e-12);
+  test = percent_t(18.0);
+  EXPECT_NEAR(0.18, test, 5.0e-12);
+}
+
+TEST_F(UnitConversion, data) {
+  EXPECT_EQ(8, (convert<byte, bit>(1)));
+
+  EXPECT_EQ(1000, (convert<kilobytes, bytes>(1)));
+  EXPECT_EQ(1000, (convert<megabytes, kilobytes>(1)));
+  EXPECT_EQ(1000, (convert<gigabytes, megabytes>(1)));
+  EXPECT_EQ(1000, (convert<terabytes, gigabytes>(1)));
+  EXPECT_EQ(1000, (convert<petabytes, terabytes>(1)));
+  EXPECT_EQ(1000, (convert<exabytes, petabytes>(1)));
+
+  EXPECT_EQ(1024, (convert<kibibytes, bytes>(1)));
+  EXPECT_EQ(1024, (convert<mebibytes, kibibytes>(1)));
+  EXPECT_EQ(1024, (convert<gibibytes, mebibytes>(1)));
+  EXPECT_EQ(1024, (convert<tebibytes, gibibytes>(1)));
+  EXPECT_EQ(1024, (convert<pebibytes, tebibytes>(1)));
+  EXPECT_EQ(1024, (convert<exbibytes, pebibytes>(1)));
+
+  EXPECT_EQ(93750000, (convert<gigabytes, kibibits>(12)));
+
+  EXPECT_EQ(1000, (convert<kilobits, bits>(1)));
+  EXPECT_EQ(1000, (convert<megabits, kilobits>(1)));
+  EXPECT_EQ(1000, (convert<gigabits, megabits>(1)));
+  EXPECT_EQ(1000, (convert<terabits, gigabits>(1)));
+  EXPECT_EQ(1000, (convert<petabits, terabits>(1)));
+  EXPECT_EQ(1000, (convert<exabits, petabits>(1)));
+
+  EXPECT_EQ(1024, (convert<kibibits, bits>(1)));
+  EXPECT_EQ(1024, (convert<mebibits, kibibits>(1)));
+  EXPECT_EQ(1024, (convert<gibibits, mebibits>(1)));
+  EXPECT_EQ(1024, (convert<tebibits, gibibits>(1)));
+  EXPECT_EQ(1024, (convert<pebibits, tebibits>(1)));
+  EXPECT_EQ(1024, (convert<exbibits, pebibits>(1)));
+
+  // Source: https://en.wikipedia.org/wiki/Binary_prefix
+  EXPECT_NEAR(percent_t(2.4), kibibyte_t(1) / kilobyte_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(4.9), mebibyte_t(1) / megabyte_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(7.4), gibibyte_t(1) / gigabyte_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(10.0), tebibyte_t(1) / terabyte_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(12.6), pebibyte_t(1) / petabyte_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(15.3), exbibyte_t(1) / exabyte_t(1) - 1, 0.005);
+}
+
+TEST_F(UnitConversion, data_transfer_rate) {
+  EXPECT_EQ(8, (convert<bytes_per_second, bits_per_second>(1)));
+
+  EXPECT_EQ(1000, (convert<kilobytes_per_second, bytes_per_second>(1)));
+  EXPECT_EQ(1000, (convert<megabytes_per_second, kilobytes_per_second>(1)));
+  EXPECT_EQ(1000, (convert<gigabytes_per_second, megabytes_per_second>(1)));
+  EXPECT_EQ(1000, (convert<terabytes_per_second, gigabytes_per_second>(1)));
+  EXPECT_EQ(1000, (convert<petabytes_per_second, terabytes_per_second>(1)));
+  EXPECT_EQ(1000, (convert<exabytes_per_second, petabytes_per_second>(1)));
+
+  EXPECT_EQ(1024, (convert<kibibytes_per_second, bytes_per_second>(1)));
+  EXPECT_EQ(1024, (convert<mebibytes_per_second, kibibytes_per_second>(1)));
+  EXPECT_EQ(1024, (convert<gibibytes_per_second, mebibytes_per_second>(1)));
+  EXPECT_EQ(1024, (convert<tebibytes_per_second, gibibytes_per_second>(1)));
+  EXPECT_EQ(1024, (convert<pebibytes_per_second, tebibytes_per_second>(1)));
+  EXPECT_EQ(1024, (convert<exbibytes_per_second, pebibytes_per_second>(1)));
+
+  EXPECT_EQ(93750000, (convert<gigabytes_per_second, kibibits_per_second>(12)));
+
+  EXPECT_EQ(1000, (convert<kilobits_per_second, bits_per_second>(1)));
+  EXPECT_EQ(1000, (convert<megabits_per_second, kilobits_per_second>(1)));
+  EXPECT_EQ(1000, (convert<gigabits_per_second, megabits_per_second>(1)));
+  EXPECT_EQ(1000, (convert<terabits_per_second, gigabits_per_second>(1)));
+  EXPECT_EQ(1000, (convert<petabits_per_second, terabits_per_second>(1)));
+  EXPECT_EQ(1000, (convert<exabits_per_second, petabits_per_second>(1)));
+
+  EXPECT_EQ(1024, (convert<kibibits_per_second, bits_per_second>(1)));
+  EXPECT_EQ(1024, (convert<mebibits_per_second, kibibits_per_second>(1)));
+  EXPECT_EQ(1024, (convert<gibibits_per_second, mebibits_per_second>(1)));
+  EXPECT_EQ(1024, (convert<tebibits_per_second, gibibits_per_second>(1)));
+  EXPECT_EQ(1024, (convert<pebibits_per_second, tebibits_per_second>(1)));
+  EXPECT_EQ(1024, (convert<exbibits_per_second, pebibits_per_second>(1)));
+
+  // Source: https://en.wikipedia.org/wiki/Binary_prefix
+  EXPECT_NEAR(percent_t(2.4),
+              kibibytes_per_second_t(1) / kilobytes_per_second_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(4.9),
+              mebibytes_per_second_t(1) / megabytes_per_second_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(7.4),
+              gibibytes_per_second_t(1) / gigabytes_per_second_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(10.0),
+              tebibytes_per_second_t(1) / terabytes_per_second_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(12.6),
+              pebibytes_per_second_t(1) / petabytes_per_second_t(1) - 1, 0.005);
+  EXPECT_NEAR(percent_t(15.3),
+              exbibytes_per_second_t(1) / exabytes_per_second_t(1) - 1, 0.005);
+}
+
+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);
+
+  // implicit conversion/arithmetic
+  EXPECT_NEAR(3.14159, constants::pi, 5.0e-6);
+  EXPECT_NEAR(6.28318531, (2 * constants::pi), 5.0e-9);
+  EXPECT_NEAR(6.28318531, (constants::pi + constants::pi), 5.0e-9);
+  EXPECT_NEAR(0.0, (constants::pi - constants::pi), 5.0e-9);
+  EXPECT_NEAR(31.00627668, units::math::cpow<3>(constants::pi), 5.0e-10);
+  EXPECT_NEAR(0.0322515344, (1.0 / units::math::cpow<3>(constants::pi)),
+              5.0e-11);
+  EXPECT_TRUE(constants::detail::PI_VAL == constants::pi);
+  EXPECT_TRUE(1.0 != constants::pi);
+  EXPECT_TRUE(4.0 > constants::pi);
+  EXPECT_TRUE(3.0 < constants::pi);
+  EXPECT_TRUE(constants::pi > 3.0);
+  EXPECT_TRUE(constants::pi < 4.0);
+
+  // explicit conversion
+  EXPECT_NEAR(3.14159, constants::pi.to<double>(), 5.0e-6);
+
+  // auto multiplication
+  EXPECT_TRUE(
+      (std::is_same<meter_t, decltype(constants::pi * meter_t(1))>::value));
+  EXPECT_TRUE(
+      (std::is_same<meter_t, decltype(meter_t(1) * constants::pi)>::value));
+
+  EXPECT_NEAR(constants::detail::PI_VAL,
+              (constants::pi * meter_t(1)).to<double>(), 5.0e-10);
+  EXPECT_NEAR(constants::detail::PI_VAL,
+              (meter_t(1) * constants::pi).to<double>(), 5.0e-10);
+
+  // explicit multiplication
+  meter_t a = constants::pi * meter_t(1);
+  meter_t b = meter_t(1) * constants::pi;
+
+  EXPECT_NEAR(constants::detail::PI_VAL, a.to<double>(), 5.0e-10);
+  EXPECT_NEAR(constants::detail::PI_VAL, b.to<double>(), 5.0e-10);
+
+  // auto division
+  EXPECT_TRUE(
+      (std::is_same<hertz_t, decltype(constants::pi / second_t(1))>::value));
+  EXPECT_TRUE(
+      (std::is_same<second_t, decltype(second_t(1) / constants::pi)>::value));
+
+  EXPECT_NEAR(constants::detail::PI_VAL,
+              (constants::pi / second_t(1)).to<double>(), 5.0e-10);
+  EXPECT_NEAR(1.0 / constants::detail::PI_VAL,
+              (second_t(1) / constants::pi).to<double>(), 5.0e-10);
+
+  // explicit
+  hertz_t c = constants::pi / second_t(1);
+  second_t d = second_t(1) / constants::pi;
+
+  EXPECT_NEAR(constants::detail::PI_VAL, c.to<double>(), 5.0e-10);
+  EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.to<double>(), 5.0e-10);
+}
+
+TEST_F(UnitConversion, constants) {
+  // Source: NIST "2014 CODATA recommended values"
+  EXPECT_NEAR(299792458, constants::c(), 5.0e-9);
+  EXPECT_NEAR(6.67408e-11, constants::G(), 5.0e-17);
+  EXPECT_NEAR(6.626070040e-34, constants::h(), 5.0e-44);
+  EXPECT_NEAR(1.2566370614e-6, constants::mu0(), 5.0e-17);
+  EXPECT_NEAR(8.854187817e-12, constants::epsilon0(), 5.0e-21);
+  EXPECT_NEAR(376.73031346177, constants::Z0(), 5.0e-12);
+  EXPECT_NEAR(8987551787.3681764, constants::k_e(), 5.0e-6);
+  EXPECT_NEAR(1.6021766208e-19, constants::e(), 5.0e-29);
+  EXPECT_NEAR(9.10938356e-31, constants::m_e(), 5.0e-40);
+  EXPECT_NEAR(1.672621898e-27, constants::m_p(), 5.0e-37);
+  EXPECT_NEAR(9.274009994e-24, constants::mu_B(), 5.0e-32);
+  EXPECT_NEAR(6.022140857e23, constants::N_A(), 5.0e14);
+  EXPECT_NEAR(8.3144598, constants::R(), 5.0e-8);
+  EXPECT_NEAR(1.38064852e-23, constants::k_B(), 5.0e-31);
+  EXPECT_NEAR(96485.33289, constants::F(), 5.0e-5);
+  EXPECT_NEAR(5.670367e-8, constants::sigma(), 5.0e-14);
+}
+
+TEST_F(UnitConversion, std_chrono) {
+  nanosecond_t a = std::chrono::nanoseconds(10);
+  EXPECT_EQ(nanosecond_t(10), a);
+  microsecond_t b = std::chrono::microseconds(10);
+  EXPECT_EQ(microsecond_t(10), b);
+  millisecond_t c = std::chrono::milliseconds(10);
+  EXPECT_EQ(millisecond_t(10), c);
+  second_t d = std::chrono::seconds(1);
+  EXPECT_EQ(second_t(1), d);
+  minute_t e = std::chrono::minutes(120);
+  EXPECT_EQ(minute_t(120), e);
+  hour_t f = std::chrono::hours(2);
+  EXPECT_EQ(hour_t(2), f);
+
+  std::chrono::nanoseconds g = nanosecond_t(100);
+  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(g).count(),
+            100);
+  std::chrono::nanoseconds h = microsecond_t(2);
+  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(h).count(),
+            2000);
+  std::chrono::nanoseconds i = millisecond_t(1);
+  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(i).count(),
+            1000000);
+  std::chrono::nanoseconds j = second_t(1);
+  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(j).count(),
+            1000000000);
+  std::chrono::nanoseconds k = minute_t(1);
+  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(k).count(),
+            60000000000);
+  std::chrono::nanoseconds l = hour_t(1);
+  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(l).count(),
+            3600000000000);
+}
+
+TEST_F(UnitConversion, squaredTemperature) {
+  using squared_celsius = units::compound_unit<squared<celsius>>;
+  using squared_celsius_t = units::unit_t<squared_celsius>;
+  const squared_celsius_t right(100);
+  const celsius_t rootRight = units::math::sqrt(right);
+  EXPECT_EQ(celsius_t(10), rootRight);
+}
+
+TEST_F(UnitMath, min) {
+  meter_t a(1);
+  foot_t c(1);
+  EXPECT_EQ(c, math::min(a, c));
+}
+
+TEST_F(UnitMath, max) {
+  meter_t a(1);
+  foot_t c(1);
+  EXPECT_EQ(a, math::max(a, c));
+}
+
+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_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_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_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));
+  EXPECT_NEAR(angle::radian_t(2).to<double>(),
+              acos(scalar_t(-0.41614683654)).to<double>(), 5.0e-11);
+  EXPECT_NEAR(
+      angle::degree_t(135).to<double>(),
+      angle::degree_t(acos(scalar_t(-0.70710678118654752440084436210485)))
+          .to<double>(),
+      5.0e-12);
+}
+
+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));
+  EXPECT_NEAR(angle::radian_t(1.14159265).to<double>(),
+              asin(scalar_t(0.90929742682)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(
+      angle::degree_t(45).to<double>(),
+      angle::degree_t(asin(scalar_t(0.70710678118654752440084436210485)))
+          .to<double>(),
+      5.0e-12);
+}
+
+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));
+  EXPECT_NEAR(angle::radian_t(-1.14159265).to<double>(),
+              atan(scalar_t(-2.18503986326)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(angle::degree_t(-45).to<double>(),
+              angle::degree_t(atan(scalar_t(-1.0))).to<double>(), 5.0e-12);
+}
+
+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_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).to<double>(),
+              atan2(scalar_t(2), scalar_t(2)).to<double>(), 5.0e-12);
+  EXPECT_NEAR(
+      angle::degree_t(45).to<double>(),
+      angle::degree_t(atan2(scalar_t(2), scalar_t(2))).to<double>(),
+      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_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).to<double>(),
+              atan2(scalar_t(1), scalar_t(std::sqrt(3))).to<double>(),
+              5.0e-12);
+  EXPECT_NEAR(angle::degree_t(30).to<double>(),
+              angle::degree_t(atan2(scalar_t(1), scalar_t(std::sqrt(3))))
+                  .to<double>(),
+              5.0e-12);
+}
+
+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_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_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_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_NEAR(angle::radian_t(1.316957896924817).to<double>(),
+              acosh(scalar_t(2.0)).to<double>(), 5.0e-11);
+  EXPECT_NEAR(angle::degree_t(75.456129290216893).to<double>(),
+              angle::degree_t(acosh(scalar_t(2.0))).to<double>(), 5.0e-12);
+}
+
+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_NEAR(angle::radian_t(1.443635475178810).to<double>(),
+              asinh(scalar_t(2)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(angle::degree_t(82.714219883108939).to<double>(),
+              angle::degree_t(asinh(scalar_t(2))).to<double>(), 5.0e-12);
+}
+
+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_NEAR(angle::radian_t(0.549306144334055).to<double>(),
+              atanh(scalar_t(0.5)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(angle::degree_t(31.472923730945389).to<double>(),
+              angle::degree_t(atanh(scalar_t(0.5))).to<double>(), 5.0e-12);
+}
+
+TEST_F(UnitMath, exp) {
+  double val = 10.0;
+  EXPECT_EQ(std::exp(val), exp(scalar_t(val)));
+}
+
+TEST_F(UnitMath, log) {
+  double val = 100.0;
+  EXPECT_EQ(std::log(val), log(scalar_t(val)));
+}
+
+TEST_F(UnitMath, log10) {
+  double val = 100.0;
+  EXPECT_EQ(std::log10(val), log10(scalar_t(val)));
+}
+
+TEST_F(UnitMath, modf) {
+  double val = 100.0;
+  double modfr1;
+  scalar_t modfr2;
+  EXPECT_EQ(std::modf(val, &modfr1), modf(scalar_t(val), &modfr2));
+  EXPECT_EQ(modfr1, modfr2);
+}
+
+TEST_F(UnitMath, exp2) {
+  double val = 10.0;
+  EXPECT_EQ(std::exp2(val), exp2(scalar_t(val)));
+}
+
+TEST_F(UnitMath, expm1) {
+  double val = 10.0;
+  EXPECT_EQ(std::expm1(val), expm1(scalar_t(val)));
+}
+
+TEST_F(UnitMath, log1p) {
+  double val = 10.0;
+  EXPECT_EQ(std::log1p(val), log1p(scalar_t(val)));
+}
+
+TEST_F(UnitMath, log2) {
+  double val = 10.0;
+  EXPECT_EQ(std::log2(val), log2(scalar_t(val)));
+}
+
+TEST_F(UnitMath, pow) {
+  bool isSame;
+  meter_t value(10.0);
+
+  auto sq = pow<2>(value);
+  EXPECT_NEAR(100.0, sq(), 5.0e-2);
+  isSame = std::is_same<decltype(sq), square_meter_t>::value;
+  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;
+  EXPECT_TRUE(isSame);
+
+  auto fourth = pow<4>(value);
+  EXPECT_NEAR(10000.0, fourth(), 5.0e-2);
+  isSame = std::is_same<
+      decltype(fourth),
+      unit_t<compound_unit<squared<meter>, squared<meter>>>>::value;
+  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_NEAR(meter_t(2.0).to<double>(),
+              sqrt(square_meter_t(4.0)).to<double>(), 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_NEAR(angle::radian_t(4.0).to<double>(),
+              sqrt(steradian_t(16.0)).to<double>(), 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));
+
+  // for rational conversion (i.e. no integral root) let's check a bunch of
+  // different ways this could go wrong
+  foot_t resultFt = sqrt(square_foot_t(10.0));
+  EXPECT_NEAR(foot_t(3.16227766017).to<double>(),
+              sqrt(square_foot_t(10.0)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(foot_t(3.16227766017).to<double>(), resultFt.to<double>(),
+              5.0e-9);
+  EXPECT_EQ(resultFt, sqrt(square_foot_t(10.0)));
+}
+
+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_NEAR(meter_t(5.0).to<double>(),
+              (hypot(meter_t(3.0), meter_t(4.0))).to<double>(), 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_NEAR(foot_t(5.0).to<double>(),
+              (hypot(foot_t(3.0), meter_t(1.2192))).to<double>(), 5.0e-9);
+}
+
+TEST_F(UnitMath, ceil) {
+  double val = 101.1;
+  EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).to<double>());
+  EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
+                            typename std::decay<decltype(
+                                ceil(meter_t(val)))>::type>::value));
+}
+
+TEST_F(UnitMath, floor) {
+  double val = 101.1;
+  EXPECT_EQ(std::floor(val), floor(scalar_t(val)));
+}
+
+TEST_F(UnitMath, fmod) {
+  EXPECT_EQ(std::fmod(100.0, 101.2),
+            fmod(meter_t(100.0), meter_t(101.2)).to<double>());
+}
+
+TEST_F(UnitMath, trunc) {
+  double val = 101.1;
+  EXPECT_EQ(std::trunc(val), trunc(scalar_t(val)));
+}
+
+TEST_F(UnitMath, round) {
+  double val = 101.1;
+  EXPECT_EQ(std::round(val), round(scalar_t(val)));
+}
+
+TEST_F(UnitMath, copysign) {
+  double sign = -1;
+  meter_t val(5.0);
+  EXPECT_EQ(meter_t(-5.0), copysign(val, sign));
+  EXPECT_EQ(meter_t(-5.0), copysign(val, angle::radian_t(sign)));
+}
+
+TEST_F(UnitMath, fdim) {
+  EXPECT_EQ(meter_t(0.0), fdim(meter_t(8.0), meter_t(10.0)));
+  EXPECT_EQ(meter_t(2.0), fdim(meter_t(10.0), meter_t(8.0)));
+  EXPECT_NEAR(meter_t(9.3904).to<double>(),
+              fdim(meter_t(10.0), foot_t(2.0)).to<double>(),
+              5.0e-320);  // not sure why they aren't comparing exactly equal,
+                          // but clearly they are.
+}
+
+TEST_F(UnitMath, fmin) {
+  EXPECT_EQ(meter_t(8.0), fmin(meter_t(8.0), meter_t(10.0)));
+  EXPECT_EQ(meter_t(8.0), fmin(meter_t(10.0), meter_t(8.0)));
+  EXPECT_EQ(foot_t(2.0), fmin(meter_t(10.0), foot_t(2.0)));
+}
+
+TEST_F(UnitMath, fmax) {
+  EXPECT_EQ(meter_t(10.0), fmax(meter_t(8.0), meter_t(10.0)));
+  EXPECT_EQ(meter_t(10.0), fmax(meter_t(10.0), meter_t(8.0)));
+  EXPECT_EQ(meter_t(10.0), fmax(meter_t(10.0), foot_t(2.0)));
+}
+
+TEST_F(UnitMath, fabs) {
+  EXPECT_EQ(meter_t(10.0), fabs(meter_t(-10.0)));
+  EXPECT_EQ(meter_t(10.0), fabs(meter_t(10.0)));
+}
+
+TEST_F(UnitMath, abs) {
+  EXPECT_EQ(meter_t(10.0), abs(meter_t(-10.0)));
+  EXPECT_EQ(meter_t(10.0), abs(meter_t(10.0)));
+}
+
+TEST_F(UnitMath, normalize) {
+  EXPECT_EQ(NormalizeAngle(radian_t(5 * wpi::math::pi)),
+      radian_t(wpi::math::pi));
+  EXPECT_EQ(NormalizeAngle(radian_t(-5 * wpi::math::pi)),
+      radian_t(wpi::math::pi));
+  EXPECT_EQ(NormalizeAngle(radian_t(wpi::math::pi / 2)),
+      radian_t(wpi::math::pi / 2));
+  EXPECT_EQ(NormalizeAngle(radian_t(-wpi::math::pi / 2)),
+      radian_t(-wpi::math::pi / 2));
+}
+
+// Constexpr
+#if !defined(_MSC_VER) || _MSC_VER > 1800
+TEST_F(Constexpr, construction) {
+  constexpr meter_t result0(0);
+  constexpr auto result1 = make_unit<meter_t>(1);
+  constexpr auto result2 = meter_t(2);
+
+  EXPECT_EQ(meter_t(0), result0);
+  EXPECT_EQ(meter_t(1), result1);
+  EXPECT_EQ(meter_t(2), result2);
+
+  EXPECT_TRUE(noexcept(result0));
+  EXPECT_TRUE(noexcept(result1));
+  EXPECT_TRUE(noexcept(result2));
+}
+
+TEST_F(Constexpr, constants) {
+  EXPECT_TRUE(noexcept(constants::c()));
+  EXPECT_TRUE(noexcept(constants::G()));
+  EXPECT_TRUE(noexcept(constants::h()));
+  EXPECT_TRUE(noexcept(constants::mu0()));
+  EXPECT_TRUE(noexcept(constants::epsilon0()));
+  EXPECT_TRUE(noexcept(constants::Z0()));
+  EXPECT_TRUE(noexcept(constants::k_e()));
+  EXPECT_TRUE(noexcept(constants::e()));
+  EXPECT_TRUE(noexcept(constants::m_e()));
+  EXPECT_TRUE(noexcept(constants::m_p()));
+  EXPECT_TRUE(noexcept(constants::mu_B()));
+  EXPECT_TRUE(noexcept(constants::N_A()));
+  EXPECT_TRUE(noexcept(constants::R()));
+  EXPECT_TRUE(noexcept(constants::k_B()));
+  EXPECT_TRUE(noexcept(constants::F()));
+  EXPECT_TRUE(noexcept(constants::sigma()));
+}
+
+TEST_F(Constexpr, arithmetic) {
+  constexpr auto result0(1_m + 1_m);
+  constexpr auto result1(1_m - 1_m);
+  constexpr auto result2(1_m * 1_m);
+  constexpr auto result3(1_m / 1_m);
+  constexpr auto result4(meter_t(1) + meter_t(1));
+  constexpr auto result5(meter_t(1) - meter_t(1));
+  constexpr auto result6(meter_t(1) * meter_t(1));
+  constexpr auto result7(meter_t(1) / meter_t(1));
+  constexpr auto result8(units::math::cpow<2>(meter_t(2)));
+  constexpr auto result9 = units::math::cpow<3>(2_m);
+  constexpr auto result10 = 2_m * 2_m;
+
+  EXPECT_TRUE(noexcept(result0));
+  EXPECT_TRUE(noexcept(result1));
+  EXPECT_TRUE(noexcept(result2));
+  EXPECT_TRUE(noexcept(result3));
+  EXPECT_TRUE(noexcept(result4));
+  EXPECT_TRUE(noexcept(result5));
+  EXPECT_TRUE(noexcept(result6));
+  EXPECT_TRUE(noexcept(result7));
+  EXPECT_TRUE(noexcept(result8));
+  EXPECT_TRUE(noexcept(result9));
+  EXPECT_TRUE(noexcept(result10));
+
+  EXPECT_EQ(8_cu_m, result9);
+  EXPECT_EQ(4_sq_m, result10);
+}
+
+TEST_F(Constexpr, realtional) {
+  constexpr bool equalityTrue = (1_m == 1_m);
+  constexpr bool equalityFalse = (1_m == 2_m);
+  constexpr bool lessThanTrue = (1_m < 2_m);
+  constexpr bool lessThanFalse = (1_m < 1_m);
+  constexpr bool lessThanEqualTrue1 = (1_m <= 1_m);
+  constexpr bool lessThanEqualTrue2 = (1_m <= 2_m);
+  constexpr bool lessThanEqualFalse = (1_m < 0_m);
+  constexpr bool greaterThanTrue = (2_m > 1_m);
+  constexpr bool greaterThanFalse = (2_m > 2_m);
+  constexpr bool greaterThanEqualTrue1 = (2_m >= 1_m);
+  constexpr bool greaterThanEqualTrue2 = (2_m >= 2_m);
+  constexpr bool greaterThanEqualFalse = (2_m > 3_m);
+
+  EXPECT_TRUE(equalityTrue);
+  EXPECT_TRUE(lessThanTrue);
+  EXPECT_TRUE(lessThanEqualTrue1);
+  EXPECT_TRUE(lessThanEqualTrue2);
+  EXPECT_TRUE(greaterThanTrue);
+  EXPECT_TRUE(greaterThanEqualTrue1);
+  EXPECT_TRUE(greaterThanEqualTrue2);
+  EXPECT_FALSE(equalityFalse);
+  EXPECT_FALSE(lessThanFalse);
+  EXPECT_FALSE(lessThanEqualFalse);
+  EXPECT_FALSE(greaterThanFalse);
+  EXPECT_FALSE(greaterThanEqualFalse);
+}
+
+TEST_F(Constexpr, stdArray) {
+  constexpr std::array<meter_t, 5> arr = {0_m, 1_m, 2_m, 3_m, 4_m};
+  constexpr bool equal = (arr[3] == 3_m);
+  EXPECT_TRUE(equal);
+}
+
+#endif
+
+TEST_F(CompileTimeArithmetic, unit_value_t) {
+  typedef unit_value_t<meters, 3, 2> mRatio;
+  EXPECT_EQ(meter_t(1.5), mRatio::value());
+}
+
+TEST_F(CompileTimeArithmetic, is_unit_value_t) {
+  typedef unit_value_t<meters, 3, 2> mRatio;
+
+  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<mRatio, meters>::value));
+  EXPECT_FALSE((traits::is_unit_value_t<meter_t, meters>::value));
+  EXPECT_FALSE((traits::is_unit_value_t<double, meters>::value));
+}
+
+TEST_F(CompileTimeArithmetic, is_unit_value_t_category) {
+  typedef unit_value_t<feet, 3, 2> mRatio;
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::length_unit, mRatio>::value));
+  EXPECT_FALSE(
+      (traits::is_unit_value_t_category<category::angle_unit, mRatio>::value));
+  EXPECT_FALSE((
+      traits::is_unit_value_t_category<category::length_unit, meter_t>::value));
+  EXPECT_FALSE(
+      (traits::is_unit_value_t_category<category::length_unit, double>::value));
+}
+
+TEST_F(CompileTimeArithmetic, unit_value_add) {
+  typedef unit_value_t<meters, 3, 2> mRatio;
+
+  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));
+
+  typedef unit_value_t<feet, 1> ftRatio;
+
+  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));
+  EXPECT_NEAR(5.92125984, sumf::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::length_unit, sumf>::value));
+
+  typedef unit_value_t<celsius, 1> cRatio;
+  typedef unit_value_t<fahrenheit, 2> fRatio;
+
+  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));
+  EXPECT_NEAR(2.11111111111, sumc::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
+                                                sumc>::value));
+
+  typedef unit_value_t<angle::radian, 1> rRatio;
+  typedef unit_value_t<angle::degree, 3> dRatio;
+
+  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));
+  EXPECT_NEAR(1.05235988, sumr::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::angle_unit, sumr>::value));
+}
+
+TEST_F(CompileTimeArithmetic, unit_value_subtract) {
+  typedef unit_value_t<meters, 3, 2> mRatio;
+
+  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));
+
+  typedef unit_value_t<feet, 1> ftRatio;
+
+  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_NEAR(-3.92125984, difff::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::length_unit, difff>::value));
+
+  typedef unit_value_t<celsius, 1> cRatio;
+  typedef unit_value_t<fahrenheit, 2> fRatio;
+
+  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_NEAR(-0.11111111111, diffc::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
+                                                diffc>::value));
+
+  typedef unit_value_t<angle::radian, 1> rRatio;
+  typedef unit_value_t<angle::degree, 3> dRatio;
+
+  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_NEAR(0.947640122, diffr::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::angle_unit, diffr>::value));
+}
+
+TEST_F(CompileTimeArithmetic, unit_value_multiply) {
+  typedef unit_value_t<meters, 2> mRatio;
+  typedef unit_value_t<feet, 656168, 100000> ftRatio;  // 2 meter
+
+  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));
+
+  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_NEAR(4.0, productM::value().to<double>(), 5.0e-7);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::area_unit, productM>::value));
+
+  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_NEAR(43.0556444224, productF::value().to<double>(), 5.0e-6);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::area_unit, productF>::value));
+
+  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));
+  EXPECT_NEAR(43.0556444224, productF2::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE((
+      traits::is_unit_value_t_category<category::area_unit, productF2>::value));
+
+  typedef unit_value_t<units::force::newton, 5> nRatio;
+
+  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));
+  EXPECT_NEAR(32.8084, productN::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().to<double>()),
+              5.0e-7);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::torque_unit,
+                                                productN>::value));
+
+  typedef unit_value_t<angle::radian, 11, 10> r1Ratio;
+  typedef unit_value_t<angle::radian, 22, 10> r2Ratio;
+
+  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_NEAR(2.42, productR::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(7944.39137,
+              (productR::value().convert<degrees_squared>().to<double>()),
+              5.0e-6);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
+                                                productR>::value));
+}
+
+TEST_F(CompileTimeArithmetic, unit_value_divide) {
+  typedef unit_value_t<meters, 2> mRatio;
+  typedef unit_value_t<feet, 656168, 100000> ftRatio;  // 2 meter
+
+  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));
+
+  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_NEAR(1, productM::value().to<double>(), 5.0e-7);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
+                                                productM>::value));
+
+  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_NEAR(1.0, productF::value().to<double>(), 5.0e-6);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
+                                                productF>::value));
+
+  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));
+  EXPECT_NEAR(1.0, productF2::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
+                                                productF2>::value));
+
+  typedef unit_value_t<seconds, 10> sRatio;
+
+  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));
+  EXPECT_NEAR(0.2, productMS::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::velocity_unit,
+                                                productMS>::value));
+
+  typedef unit_value_t<angle::radian, 20> rRatio;
+
+  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));
+  EXPECT_NEAR(2, productRS::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(114.592,
+              (productRS::value().convert<degrees_per_second>().to<double>()),
+              5.0e-4);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::angular_velocity_unit,
+                                                productRS>::value));
+}
+
+TEST_F(CompileTimeArithmetic, unit_value_power) {
+  typedef unit_value_t<meters, 2> mRatio;
+
+  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_NEAR(4, sq::value().to<double>(), 5.0e-8);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::area_unit, sq>::value));
+
+  typedef unit_value_t<angle::radian, 18, 10> rRatio;
+
+  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_NEAR(3.24, sqr::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(10636.292574038049895092690529904,
+              (sqr::value().convert<degrees_squared>().to<double>()), 5.0e-10);
+  EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
+                                                sqr>::value));
+}
+
+TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
+  typedef unit_value_t<square_meters, 10> mRatio;
+
+  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_NEAR(3.16227766017, root::value().to<double>(), 5.0e-9);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::length_unit, root>::value));
+
+  typedef unit_value_t<hectare, 51, 7> hRatio;
+
+  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_NEAR(2.69920623253, rooth::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(269.920623, rooth::value().convert<meters>().to<double>(),
+              5.0e-6);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::length_unit, rooth>::value));
+
+  typedef unit_value_t<steradian, 18, 10> rRatio;
+
+  using rootr = unit_value_sqrt<rRatio>;
+  EXPECT_TRUE((traits::is_angle_unit<decltype(rootr::value())>::value));
+  EXPECT_NEAR(1.3416407865, rootr::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(76.870352574,
+              rootr::value().convert<angle::degrees>().to<double>(), 5.0e-6);
+  EXPECT_TRUE(
+      (traits::is_unit_value_t_category<category::angle_unit, rootr>::value));
+}
+
+TEST_F(CaseStudies, radarRangeEquation) {
+  watt_t P_t;            // transmit power
+  scalar_t G;            // gain
+  meter_t lambda;        // wavelength
+  square_meter_t sigma;  // radar cross section
+  meter_t R;             // range
+  kelvin_t T_s;          // system noise temp
+  hertz_t B_n;           // bandwidth
+  scalar_t L;            // loss
+
+  P_t = megawatt_t(1.4);
+  G = dB_t(33.0);
+  lambda = constants::c / megahertz_t(2800);
+  sigma = square_meter_t(1.0);
+  R = meter_t(111000.0);
+  T_s = kelvin_t(950.0);
+  B_n = megahertz_t(1.67);
+  L = dB_t(8.0);
+
+  scalar_t SNR = (P_t * math::pow<2>(G) * math::pow<2>(lambda) * sigma) /
+                 (math::pow<3>(4 * constants::pi) * math::pow<4>(R) *
+                  constants::k_B * T_s * B_n * L);
+
+  EXPECT_NEAR(1.535, SNR(), 5.0e-4);
+}
+
+TEST_F(CaseStudies, pythagoreanTheorum) {
+  EXPECT_EQ(meter_t(3), RightTriangle::a::value());
+  EXPECT_EQ(meter_t(4), RightTriangle::b::value());
+  EXPECT_EQ(meter_t(5), RightTriangle::c::value());
+  EXPECT_TRUE(pow<2>(RightTriangle::a::value()) +
+                  pow<2>(RightTriangle::b::value()) ==
+              pow<2>(RightTriangle::c::value()));
+}

wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp

diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
new file mode 100644
index 0000000..79f1a6d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/ControlAffinePlantInversionFeedforward.h"
+#include "units/time.h"
+
+namespace frc {
+
+Eigen::Matrix<double, 2, 1> Dynamics(const Eigen::Matrix<double, 2, 1>& x,
+                                     const Eigen::Matrix<double, 1, 1>& u) {
+  Eigen::Matrix<double, 2, 1> result;
+
+  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x) +
+           (frc::MakeMatrix<2, 1>(0.0, 1.0) * u);
+
+  return result;
+}
+
+Eigen::Matrix<double, 2, 1> StateDynamics(
+    const Eigen::Matrix<double, 2, 1>& x) {
+  Eigen::Matrix<double, 2, 1> result;
+
+  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x);
+
+  return result;
+}
+
+TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
+  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&,
+                                            const Eigen::Matrix<double, 1, 1>&)>
+      modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
+
+  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
+      modelDynamics, units::second_t(0.02)};
+
+  Eigen::Matrix<double, 2, 1> r;
+  r << 2, 2;
+  Eigen::Matrix<double, 2, 1> nextR;
+  nextR << 3, 3;
+
+  EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
+}
+
+TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
+  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&)>
+      modelDynamics = [](auto& x) { return StateDynamics(x); };
+
+  Eigen::Matrix<double, 2, 1> B;
+  B << 0, 1;
+
+  frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
+      modelDynamics, B, units::second_t(0.02)};
+
+  Eigen::Matrix<double, 2, 1> r;
+  r << 2, 2;
+  Eigen::Matrix<double, 2, 1> nextR;
+  nextR << 3, 3;
+
+  EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
+}
+
+}  // namespace frc

wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp

diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
new file mode 100644
index 0000000..abc22d9
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "units/time.h"
+
+namespace frc {
+
+TEST(LinearPlantInversionFeedforwardTest, Calculate) {
+  Eigen::Matrix<double, 2, 2> A;
+  A << 1, 0, 0, 1;
+
+  Eigen::Matrix<double, 2, 1> B;
+  B << 0, 1;
+
+  frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
+                                                         units::second_t(0.02)};
+
+  Eigen::Matrix<double, 2, 1> r;
+  r << 2, 2;
+  Eigen::Matrix<double, 2, 1> nextR;
+  nextR << 3, 3;
+
+  EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
+}
+
+}  // namespace frc

wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp

diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
new file mode 100644
index 0000000..eecaf34
--- /dev/null
+++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/time.h"
+
+namespace frc {
+
+TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(2);
+
+    // Carriage mass
+    constexpr auto m = 5_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.0181864_m;
+
+    // Gear ratio
+    constexpr double G = 40.0 / 40.0;
+
+    return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+  }();
+  LinearQuadraticRegulator<2, 1> controller{
+      plant, {0.02, 0.4}, {12.0}, 0.00505_s};
+
+  EXPECT_NEAR(522.15314269, controller.K(0, 0), 1e-6);
+  EXPECT_NEAR(38.20138596, controller.K(0, 1), 1e-6);
+}
+
+TEST(LinearQuadraticRegulatorTest, ArmGains) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(2);
+
+    // Carriage mass
+    constexpr auto m = 4_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.4_m;
+
+    // Gear ratio
+    constexpr double G = 100.0;
+
+    return frc::LinearSystemId::SingleJointedArmSystem(
+        motors, 1.0 / 3.0 * m * r * r, G);
+  }();
+
+  LinearQuadraticRegulator<2, 1> controller{
+      plant, {0.01745, 0.08726}, {12.0}, 0.00505_s};
+
+  EXPECT_NEAR(19.16, controller.K(0, 0), 1e-1);
+  EXPECT_NEAR(3.32, controller.K(0, 1), 1e-1);
+}
+
+TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(4);
+
+    // Carriage mass
+    constexpr auto m = 8_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.75_in;
+
+    // Gear ratio
+    constexpr double G = 14.67;
+
+    return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+  }();
+  LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.020_s};
+
+  EXPECT_NEAR(10.38, controller.K(0, 0), 1e-1);
+  EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
+}
+
+}  // namespace frc

wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp

diff --git a/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp
new file mode 100644
index 0000000..edcb772
--- /dev/null
+++ b/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp
@@ -0,0 +1,76 @@
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+
+#include <gtest/gtest.h>
+
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/math/autodiff.h"
+
+using Eigen::MatrixXd;
+
+namespace drake {
+namespace math {
+namespace {
+void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
+                        const Eigen::Ref<const MatrixXd>& B,
+                        const Eigen::Ref<const MatrixXd>& Q,
+                        const Eigen::Ref<const MatrixXd>& R) {
+  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
+  // Check that X is positive semi-definite.
+  EXPECT_TRUE(
+      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
+  int n = X.rows();
+  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
+  for (int i = 0; i < n; i++) {
+    EXPECT_GE(es.eigenvalues()[i], 0);
+  }
+  // Check that X is the solution to the discrete time ARE.
+  MatrixXd Y = A.transpose() * X * A - X -
+               A.transpose() * X * B * (B.transpose() * X * B + R).inverse() *
+                   B.transpose() * X * A +
+               Q;
+  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
+                              MatrixCompareType::absolute));
+}
+
+GTEST_TEST(DARE, SolveDAREandVerify) {
+  // Test 1: non-invertible A
+  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+  // Riccati Equation"
+  int n1 = 4, m1 = 1;
+  MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
+  A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
+  B1 << 0, 0, 0, 1;
+  Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
+  R1 << 0.25;
+  SolveDAREandVerify(A1, B1, Q1, R1);
+  // Test 2: invertible A
+  int n2 = 2, m2 = 1;
+  MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
+  A2 << 1, 1, 0, 1;
+  B2 << 0, 1;
+  Q2 << 1, 0, 0, 0;
+  R2 << 0.3;
+  SolveDAREandVerify(A2, B2, Q2, R2);
+  // Test 3: the first generalized eigenvalue of (S,T) is stable
+  int n3 = 2, m3 = 1;
+  MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
+  A3 << 0, 1, 0, 0;
+  B3 << 0, 1;
+  Q3 << 1, 0, 0, 1;
+  R3 << 1;
+  SolveDAREandVerify(A3, B3, Q3, R3);
+  // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
+  int n4 = 2, m4 = 2;
+  MatrixXd A4(n4, n4), B4(n4, m4), Q4(n4, n4), R4(m4, m4);
+  A4 << 1, 0, 0, 1;
+  B4 << 1, 0, 0, 1;
+  Q4 << 1, 0, 0, 1;
+  R4 << 1, 0, 0, 1;
+  SolveDAREandVerify(A4, B4, Q4, R4);
+}
+}  // namespace
+}  // namespace math
+}  // namespace drake

wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp

diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
new file mode 100644
index 0000000..387593b
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -0,0 +1,172 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <array>
+#include <cmath>
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/ExtendedKalmanFilter.h"
+#include "frc/system/NumericalJacobian.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "units/moment_of_inertia.h"
+
+namespace {
+
+Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
+                                     const Eigen::Matrix<double, 2, 1>& u) {
+  auto motors = frc::DCMotor::CIM(2);
+
+  // constexpr double Glow = 15.32;       // Low gear ratio
+  constexpr double Ghigh = 7.08;       // High gear ratio
+  constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
+  constexpr auto r = 0.0746125_m;      // Wheel radius
+  constexpr auto m = 63.503_kg;        // Robot mass
+  constexpr auto J = 5.6_kg_sq_m;      // Robot moment of inertia
+
+  auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
+            (motors.Kv * motors.R * units::math::pow<2>(r));
+  auto C2 = Ghigh * motors.Kt / (motors.R * r);
+  auto k1 = (1 / m + units::math::pow<2>(rb) / J);
+  auto k2 = (1 / m - units::math::pow<2>(rb) / J);
+
+  units::meters_per_second_t vl{x(3)};
+  units::meters_per_second_t vr{x(4)};
+  units::volt_t Vl{u(0)};
+  units::volt_t Vr{u(1)};
+
+  Eigen::Matrix<double, 5, 1> result;
+  auto v = 0.5 * (vl + vr);
+  result(0) = v.to<double>() * std::cos(x(2));
+  result(1) = v.to<double>() * std::sin(x(2));
+  result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
+  result(3) =
+      k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
+      k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
+  result(4) =
+      k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
+      k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
+  return result;
+}
+
+Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
+    const Eigen::Matrix<double, 5, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
+  static_cast<void>(u);
+  Eigen::Matrix<double, 3, 1> y;
+  y << x(2), x(3), x(4);
+  return y;
+}
+
+Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
+    const Eigen::Matrix<double, 5, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
+  static_cast<void>(u);
+  Eigen::Matrix<double, 5, 1> y;
+  y << x(0), x(1), x(2), x(3), x(4);
+  return y;
+}
+}  // namespace
+
+TEST(ExtendedKalmanFilterTest, Init) {
+  constexpr auto dt = 0.00505_s;
+
+  frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
+                                              LocalMeasurementModel,
+                                              {0.5, 0.5, 10.0, 1.0, 1.0},
+                                              {0.0001, 0.01, 0.01},
+                                              dt};
+  Eigen::Matrix<double, 2, 1> u;
+  u << 12.0, 12.0;
+  observer.Predict(u, dt);
+
+  auto localY = LocalMeasurementModel(observer.Xhat(), u);
+  observer.Correct(u, localY);
+
+  auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
+  auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+}
+
+TEST(ExtendedKalmanFilterTest, Convergence) {
+  constexpr auto dt = 0.00505_s;
+  constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
+
+  frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
+                                              LocalMeasurementModel,
+                                              {0.5, 0.5, 10.0, 1.0, 1.0},
+                                              {0.0001, 0.5, 0.5},
+                                              dt};
+
+  auto waypoints =
+      std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+
+  Eigen::Matrix<double, 5, 1> nextR;
+  Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+
+  auto B = frc::NumericalJacobianU<5, 5, 2>(
+      Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
+      Eigen::Matrix<double, 2, 1>::Zero());
+
+  observer.SetXhat(frc::MakeMatrix<5, 1>(
+      trajectory.InitialPose().Translation().X().to<double>(),
+      trajectory.InitialPose().Translation().Y().to<double>(),
+      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
+    auto ref = trajectory.Sample(dt * i);
+    units::meters_per_second_t vl =
+        ref.velocity * (1 - (ref.curvature * rb).to<double>());
+    units::meters_per_second_t vr =
+        ref.velocity * (1 + (ref.curvature * rb).to<double>());
+
+    nextR(0) = ref.pose.Translation().X().to<double>();
+    nextR(1) = ref.pose.Translation().Y().to<double>();
+    nextR(2) = ref.pose.Rotation().Radians().to<double>();
+    nextR(3) = vl.to<double>();
+    nextR(4) = vr.to<double>();
+
+    auto localY =
+        LocalMeasurementModel(nextR, Eigen::Matrix<double, 2, 1>::Zero());
+    observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+
+    Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
+    u = B.householderQr().solve(
+        rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+
+    observer.Predict(u, dt);
+
+    r = nextR;
+  }
+
+  auto localY = LocalMeasurementModel(observer.Xhat(), u);
+  observer.Correct(u, localY);
+
+  auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
+  auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+
+  auto finalPosition = trajectory.Sample(trajectory.TotalTime());
+  ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
+              observer.Xhat(0), 1.0);
+  ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
+              observer.Xhat(1), 1.0);
+  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
+              observer.Xhat(2), 1.0);
+  ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
+  ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
+}

wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp

diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
new file mode 100644
index 0000000..53c54ef
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <array>
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/estimator/KalmanFilter.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/moment_of_inertia.h"
+#include "units/time.h"
+
+TEST(KalmanFilterTest, Flywheel) {
+  auto motor = frc::DCMotor::NEO();
+  auto flywheel = frc::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
+  frc::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
+}

wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp

diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
new file mode 100644
index 0000000..ace5e79
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/MerweScaledSigmaPoints.h"
+
+namespace drake {
+namespace math {
+namespace {
+TEST(MerweScaledSigmaPointsTest, TestZeroMean) {
+  frc::MerweScaledSigmaPoints<2> sigmaPoints;
+  auto points =
+      sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(0.0, 0.0),
+                              frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0));
+
+  EXPECT_TRUE(
+      (points - frc::MakeMatrix<2, 5>(0.0, 0.00173205, 0.0, -0.00173205, 0.0,
+                                      0.0, 0.0, 0.00173205, 0.0, -0.00173205))
+          .norm() < 1e-3);
+}
+
+TEST(MerweScaledSigmaPointsTest, TestNonzeroMean) {
+  frc::MerweScaledSigmaPoints<2> sigmaPoints;
+  auto points =
+      sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(1.0, 2.0),
+                              frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 10.0));
+
+  EXPECT_TRUE(
+      (points - frc::MakeMatrix<2, 5>(1.0, 1.00173205, 1.0, 0.998268, 1.0, 2.0,
+                                      2.0, 2.00548, 2.0, 1.99452))
+          .norm() < 1e-3);
+}
+}  // namespace
+}  // namespace math
+}  // namespace drake

wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp

diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
new file mode 100644
index 0000000..0017b42
--- /dev/null
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -0,0 +1,176 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <array>
+#include <cmath>
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/system/NumericalJacobian.h"
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "units/moment_of_inertia.h"
+
+namespace {
+
+Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
+                                     const Eigen::Matrix<double, 2, 1>& u) {
+  auto motors = frc::DCMotor::CIM(2);
+
+  // constexpr double Glow = 15.32;       // Low gear ratio
+  constexpr double Ghigh = 7.08;       // High gear ratio
+  constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
+  constexpr auto r = 0.0746125_m;      // Wheel radius
+  constexpr auto m = 63.503_kg;        // Robot mass
+  constexpr auto J = 5.6_kg_sq_m;      // Robot moment of inertia
+
+  auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
+            (motors.Kv * motors.R * units::math::pow<2>(r));
+  auto C2 = Ghigh * motors.Kt / (motors.R * r);
+  auto k1 = (1 / m + units::math::pow<2>(rb) / J);
+  auto k2 = (1 / m - units::math::pow<2>(rb) / J);
+
+  units::meters_per_second_t vl{x(3)};
+  units::meters_per_second_t vr{x(4)};
+  units::volt_t Vl{u(0)};
+  units::volt_t Vr{u(1)};
+
+  Eigen::Matrix<double, 5, 1> result;
+  auto v = 0.5 * (vl + vr);
+  result(0) = v.to<double>() * std::cos(x(2));
+  result(1) = v.to<double>() * std::sin(x(2));
+  result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
+  result(3) =
+      k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
+      k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
+  result(4) =
+      k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
+      k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
+  return result;
+}
+
+Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
+    const Eigen::Matrix<double, 5, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
+  static_cast<void>(u);
+  Eigen::Matrix<double, 3, 1> y;
+  y << x(2), x(3), x(4);
+  return y;
+}
+
+Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
+    const Eigen::Matrix<double, 5, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
+  static_cast<void>(u);
+  Eigen::Matrix<double, 5, 1> y;
+  y << x(0), x(1), x(2), x(3), x(4);
+  return y;
+}
+}  // namespace
+
+TEST(UnscentedKalmanFilterTest, Init) {
+  constexpr auto dt = 0.00505_s;
+
+  frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
+                                               LocalMeasurementModel,
+                                               {0.5, 0.5, 10.0, 1.0, 1.0},
+                                               {0.0001, 0.01, 0.01},
+                                               dt};
+  Eigen::Matrix<double, 2, 1> u;
+  u << 12.0, 12.0;
+  observer.Predict(u, dt);
+
+  auto localY = LocalMeasurementModel(observer.Xhat(), u);
+  observer.Correct(u, localY);
+
+  auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
+  auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+}
+
+TEST(UnscentedKalmanFilterTest, Convergence) {
+  constexpr auto dt = 0.00505_s;
+  constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
+
+  frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
+                                               LocalMeasurementModel,
+                                               {0.5, 0.5, 10.0, 1.0, 1.0},
+                                               {0.0001, 0.5, 0.5},
+                                               dt};
+
+  auto waypoints =
+      std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+
+  Eigen::Matrix<double, 5, 1> nextR;
+  Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+
+  auto B = frc::NumericalJacobianU<5, 5, 2>(
+      Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
+      Eigen::Matrix<double, 2, 1>::Zero());
+
+  observer.SetXhat(frc::MakeMatrix<5, 1>(
+      trajectory.InitialPose().Translation().X().to<double>(),
+      trajectory.InitialPose().Translation().Y().to<double>(),
+      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+
+  auto trueXhat = observer.Xhat();
+
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
+    auto ref = trajectory.Sample(dt * i);
+    units::meters_per_second_t vl =
+        ref.velocity * (1 - (ref.curvature * rb).to<double>());
+    units::meters_per_second_t vr =
+        ref.velocity * (1 + (ref.curvature * rb).to<double>());
+
+    nextR(0) = ref.pose.Translation().X().to<double>();
+    nextR(1) = ref.pose.Translation().Y().to<double>();
+    nextR(2) = ref.pose.Rotation().Radians().to<double>();
+    nextR(3) = vl.to<double>();
+    nextR(4) = vr.to<double>();
+
+    auto localY =
+        LocalMeasurementModel(trueXhat, Eigen::Matrix<double, 2, 1>::Zero());
+    observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+
+    Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
+    u = B.householderQr().solve(
+        rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+
+    observer.Predict(u, dt);
+
+    r = nextR;
+    trueXhat = frc::RungeKutta(Dynamics, trueXhat, u, dt);
+  }
+
+  auto localY = LocalMeasurementModel(trueXhat, u);
+  observer.Correct(u, localY);
+
+  auto globalY = GlobalMeasurementModel(trueXhat, u);
+  auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+
+  auto finalPosition = trajectory.Sample(trajectory.TotalTime());
+  ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
+              observer.Xhat(0), 1.0);
+  ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
+              observer.Xhat(1), 1.0);
+  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
+              observer.Xhat(2), 1.0);
+  ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
+  ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
+}

wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp

diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
new file mode 100644
index 0000000..620c9d3
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Pose2dTest, TransformBy) {
+  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
+  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+
+  const auto transformed = initial + transform;
+
+  EXPECT_NEAR(transformed.X().to<double>(), 1 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Y().to<double>(), 2 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
+}
+
+TEST(Pose2dTest, RelativeTo) {
+  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+  const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+  EXPECT_NEAR(finalRelativeToInitial.X().to<double>(), 5.0 * std::sqrt(2.0),
+              kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
+              kEpsilon);
+}
+
+TEST(Pose2dTest, Equality) {
+  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+  const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
+  EXPECT_TRUE(a == b);
+}
+
+TEST(Pose2dTest, Inequality) {
+  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+  const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
+  EXPECT_TRUE(a != b);
+}
+
+TEST(Pose2dTest, Minus) {
+  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+  const auto transform = final - initial;
+
+  EXPECT_NEAR(transform.X().to<double>(), 5.0 * std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transform.Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}

wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp

diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
new file mode 100644
index 0000000..a29371f
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Rotation2dTest, RadiansToDegrees) {
+  const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
+  const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
+
+  EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
+  EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, DegreesToRadians) {
+  const auto one = Rotation2d(45.0_deg);
+  const auto two = Rotation2d(30.0_deg);
+
+  EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
+  EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByFromZero) {
+  const Rotation2d zero;
+  auto sum = zero + Rotation2d(90.0_deg);
+
+  EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
+  EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByNonZero) {
+  auto rot = Rotation2d(90.0_deg);
+  rot += Rotation2d(30.0_deg);
+
+  EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Minus) {
+  const auto one = Rotation2d(70.0_deg);
+  const auto two = Rotation2d(30.0_deg);
+
+  EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Equality) {
+  const auto one = Rotation2d(43_deg);
+  const auto two = Rotation2d(43_deg);
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Rotation2dTest, Inequality) {
+  const auto one = Rotation2d(43_deg);
+  const auto two = Rotation2d(43.5_deg);
+  EXPECT_TRUE(one != two);
+}

wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp

diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
new file mode 100644
index 0000000..b302fad
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Transform2dTest, Inverse) {
+  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
+  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+
+  auto transformed = initial + transform;
+  auto untransformed = transformed + transform.Inverse();
+
+  EXPECT_NEAR(initial.X().to<double>(), untransformed.X().to<double>(),
+              kEpsilon);
+  EXPECT_NEAR(initial.Y().to<double>(), untransformed.Y().to<double>(),
+              kEpsilon);
+  EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
+              untransformed.Rotation().Degrees().to<double>(), kEpsilon);
+}

wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp

diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
new file mode 100644
index 0000000..8e487f3
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Translation2dTest, Sum) {
+  const Translation2d one{1.0_m, 3.0_m};
+  const Translation2d two{2.0_m, 5.0_m};
+
+  const auto sum = one + two;
+
+  EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
+  EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Difference) {
+  const Translation2d one{1.0_m, 3.0_m};
+  const Translation2d two{2.0_m, 5.0_m};
+
+  const auto difference = one - two;
+
+  EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
+  EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
+}
+
+TEST(Translation2dTest, RotateBy) {
+  const Translation2d another{3.0_m, 0.0_m};
+  const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
+
+  EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Multiplication) {
+  const Translation2d original{3.0_m, 5.0_m};
+  const auto mult = original * 3;
+
+  EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
+  EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
+}
+
+TEST(Translation2d, Division) {
+  const Translation2d original{3.0_m, 5.0_m};
+  const auto div = original / 2;
+
+  EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
+  EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
+}
+
+TEST(Translation2dTest, Norm) {
+  const Translation2d one{3.0_m, 5.0_m};
+  EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
+}
+
+TEST(Translation2dTest, Distance) {
+  const Translation2d one{1_m, 1_m};
+  const Translation2d two{6_m, 6_m};
+  EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
+}
+
+TEST(Translation2dTest, UnaryMinus) {
+  const Translation2d original{-4.5_m, 7_m};
+  const auto inverted = -original;
+
+  EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
+  EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
+}
+
+TEST(Translation2dTest, Equality) {
+  const Translation2d one{9_m, 5.5_m};
+  const Translation2d two{9_m, 5.5_m};
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Translation2dTest, Inequality) {
+  const Translation2d one{9_m, 5.5_m};
+  const Translation2d two{9_m, 5.7_m};
+  EXPECT_TRUE(one != two);
+}
+
+TEST(Translation2dTest, PolarConstructor) {
+  Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
+  EXPECT_NEAR(one.X().to<double>(), 1.0, kEpsilon);
+  EXPECT_NEAR(one.Y().to<double>(), 1.0, kEpsilon);
+
+  Translation2d two{2_m, Rotation2d(60_deg)};
+  EXPECT_NEAR(two.X().to<double>(), 1.0, kEpsilon);
+  EXPECT_NEAR(two.Y().to<double>(), std::sqrt(3.0), kEpsilon);
+}

wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp

diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
new file mode 100644
index 0000000..4766bd4
--- /dev/null
+++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Twist2dTest, Straight) {
+  const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
+  const auto straightPose = Pose2d().Exp(straight);
+
+  EXPECT_NEAR(straightPose.X().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, QuarterCircle) {
+  const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
+                              units::radian_t(wpi::math::pi / 2.0)};
+  const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+
+  EXPECT_NEAR(quarterCirclePose.X().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Y().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
+              kEpsilon);
+}
+
+TEST(Twist2dTest, DiagonalNoDtheta) {
+  const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
+  const auto diagonalPose = Pose2d().Exp(diagonal);
+
+  EXPECT_NEAR(diagonalPose.X().to<double>(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Y().to<double>(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, Equality) {
+  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+  const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Twist2dTest, Inequality) {
+  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+  const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
+  EXPECT_TRUE(one != two);
+}
+
+TEST(Twist2dTest, Pose2dLog) {
+  const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
+  const Pose2d start{};
+
+  const auto twist = start.Log(end);
+
+  EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
+  EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp

diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
new file mode 100644
index 0000000..864860a
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(ChassisSpeeds, FieldRelativeConstruction) {
+  const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+      1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
+
+  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
+  EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
+  EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp

diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
new file mode 100644
index 0000000..7c1a28d
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "gtest/gtest.h"
+#include "units/angular_velocity.h"
+#include "units/length.h"
+#include "units/velocity.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
+  const DifferentialDriveKinematics kinematics{0.381_m * 2};
+  const ChassisSpeeds chassisSpeeds;
+  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+  EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
+  const DifferentialDriveKinematics kinematics{0.381_m * 2};
+  const DifferentialDriveWheelSpeeds wheelSpeeds;
+  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
+  const DifferentialDriveKinematics kinematics{0.381_m * 2};
+  const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
+  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+  EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
+  const DifferentialDriveKinematics kinematics{0.381_m * 2};
+  const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
+  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
+  const DifferentialDriveKinematics kinematics{0.381_m * 2};
+  const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
+                                    units::radians_per_second_t{wpi::math::pi}};
+  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+  EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
+  const DifferentialDriveKinematics kinematics{0.381_m * 2};
+  const DifferentialDriveWheelSpeeds wheelSpeeds{
+      units::meters_per_second_t(+0.381 * wpi::math::pi),
+      units::meters_per_second_t(-0.381 * wpi::math::pi)};
+  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp

diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
new file mode 100644
index 0000000..89d65ea
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+using namespace frc;
+
+TEST(DifferentialDriveOdometry, EncoderDistances) {
+  DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
+
+  const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
+                                     units::meter_t(5 * wpi::math::pi));
+
+  EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp

diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
new file mode 100644
index 0000000..cb03d97
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -0,0 +1,230 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "gtest/gtest.h"
+#include "units/angular_velocity.h"
+
+using namespace frc;
+
+class MecanumDriveKinematicsTest : public ::testing::Test {
+ protected:
+  Translation2d m_fl{12_m, 12_m};
+  Translation2d m_fr{12_m, -12_m};
+  Translation2d m_bl{-12_m, 12_m};
+  Translation2d m_br{-12_m, -12_m};
+
+  MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
+};
+
+TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
+  ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
+  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+  /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
+  */
+
+  EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
+  EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
+  EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
+  EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
+  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
+                                      3.536_mps};
+  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  /*
+  By equation (13.13) of the state-space-guide, the chassis motion from wheel
+  velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
+  [[5][0][0]]
+  */
+
+  EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
+  ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
+  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+  /*
+  By equation (13.12) of the state-space-guide, the wheel speeds should
+  be as follows:
+  velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
+  */
+
+  EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
+  EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
+  EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
+  EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
+  MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
+                                      -2.828427_mps};
+  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
+    [[5][0][0]]
+  */
+
+  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
+  EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
+  ChassisSpeeds speeds{0_mps, 0_mps,
+                       units::radians_per_second_t(2 * wpi::math::pi)};
+  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+  /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
+  */
+
+  EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
+  EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
+  EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
+  EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
+  MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
+                                      -106.62919_mps, 106.62919_mps};
+  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
+    be [[0][0][2pi]]
+  */
+
+  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
+  EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
+  ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
+  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+  /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
+  */
+
+  EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
+  EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
+  EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
+  EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
+  MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
+                                      -13.435_mps, 16.26_mps};
+
+  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  /*
+    By equation (13.13) of the state-space-guide, the chassis motion from wheel
+    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
+    [[2][3][1]]
+  */
+
+  EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
+  EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
+  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
+  ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
+  auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
+
+  /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
+  */
+
+  EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
+  EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
+  EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
+  EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
+  MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
+                                      33.941_mps};
+  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  /*
+    By equation (13.13) of the state-space-guide, the chassis motion from the
+    wheel velocities should be [[12][-12][1]]
+  */
+
+  EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
+  EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
+  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
+       OffCenterTranslationRotationInverseKinematics) {
+  ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
+  auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
+
+  /*
+    By equation (13.12) of the state-space-guide, the wheel speeds should
+    be as follows:
+    velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
+  */
+  EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
+  EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
+  EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
+  EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
+       OffCenterTranslationRotationForwardKinematics) {
+  MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
+                                      36.06_mps};
+  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  /*
+    By equation (13.13) of the state-space-guide, the chassis motion from the
+    wheel velocities should be [[17][-10][1]]
+  */
+
+  EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
+  EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
+  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
+  MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
+  wheelSpeeds.Normalize(5.5_mps);
+
+  double kFactor = 5.5 / 7.0;
+
+  EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
+}

wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp

diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
new file mode 100644
index 0000000..cb85ec7
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class MecanumDriveOdometryTest : public ::testing::Test {
+ protected:
+  Translation2d m_fl{12_m, 12_m};
+  Translation2d m_fr{12_m, -12_m};
+  Translation2d m_bl{-12_m, 12_m};
+  Translation2d m_br{-12_m, -12_m};
+
+  MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
+  MecanumDriveOdometry odometry{kinematics, 0_rad};
+};
+
+TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
+  odometry.ResetPosition(Pose2d(), 0_rad);
+  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
+                                      3.536_mps};
+
+  odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
+  auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
+
+  EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, TwoIterations) {
+  odometry.ResetPosition(Pose2d(), 0_rad);
+  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
+  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
+
+  EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
+  EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
+  odometry.ResetPosition(Pose2d(), 0_rad);
+  MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
+                                 39.986_mps};
+  odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
+  auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
+
+  EXPECT_NEAR(pose.X().to<double>(), 12, 0.01);
+  EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
+  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
+  odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+  odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
+  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
+
+  EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
+  EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}

wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
new file mode 100644
index 0000000..368dbaf
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "gtest/gtest.h"
+#include "units/angular_velocity.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 0.1;
+
+class SwerveDriveKinematicsTest : public ::testing::Test {
+ protected:
+  Translation2d m_fl{12_m, 12_m};
+  Translation2d m_fr{12_m, -12_m};
+  Translation2d m_bl{-12_m, 12_m};
+  Translation2d m_br{-12_m, -12_m};
+
+  SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
+};
+
+TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
+  ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
+
+  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+  EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+
+  EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
+  SwerveModuleState state{5.0_mps, Rotation2d()};
+
+  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
+  ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
+  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+  EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+
+  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
+  SwerveModuleState state{5_mps, Rotation2d(90_deg)};
+  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
+  ChassisSpeeds speeds{0_mps, 0_mps,
+                       units::radians_per_second_t(2 * wpi::math::pi)};
+  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+  EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
+  EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
+  EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
+  EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
+
+  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
+  SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
+  SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
+  SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
+  SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
+
+  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
+  ChassisSpeeds speeds{0_mps, 0_mps,
+                       units::radians_per_second_t(2 * wpi::math::pi)};
+  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
+
+  EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
+  EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
+  EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
+
+  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
+  SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
+  SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
+  SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
+  SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
+
+  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+       OffCenterCORRotationAndTranslationInverseKinematics) {
+  ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
+  auto [fl, fr, bl, br] =
+      m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
+
+  EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
+  EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
+  EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
+  EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
+
+  EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+       OffCenterCORRotationAndTranslationForwardKinematics) {
+  SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
+  SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
+  SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
+  SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
+
+  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
+  SwerveModuleState state1{5.0_mps, Rotation2d()};
+  SwerveModuleState state2{6.0_mps, Rotation2d()};
+  SwerveModuleState state3{4.0_mps, Rotation2d()};
+  SwerveModuleState state4{7.0_mps, Rotation2d()};
+
+  std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+  SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
+
+  double kFactor = 5.5 / 7.0;
+
+  EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
+}

wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
new file mode 100644
index 0000000..40207a1
--- /dev/null
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 0.01;
+
+class SwerveDriveOdometryTest : public ::testing::Test {
+ protected:
+  Translation2d m_fl{12_m, 12_m};
+  Translation2d m_fr{12_m, -12_m};
+  Translation2d m_bl{-12_m, 12_m};
+  Translation2d m_br{-12_m, -12_m};
+
+  SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
+  SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
+};
+
+TEST_F(SwerveDriveOdometryTest, TwoIterations) {
+  SwerveModuleState state{5_mps, Rotation2d()};
+
+  m_odometry.ResetPosition(Pose2d(), 0_rad);
+  m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
+                            SwerveModuleState(), SwerveModuleState(),
+                            SwerveModuleState());
+  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
+                                        state, state);
+
+  EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
+
+TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
+  SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
+  SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
+  SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
+  SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
+
+  SwerveModuleState zero{0_mps, Rotation2d()};
+
+  m_odometry.ResetPosition(Pose2d(), 0_rad);
+  m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
+  auto pose =
+      m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
+
+  EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
+  EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
+  EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
+
+TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
+  m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+  SwerveModuleState state{5_mps, Rotation2d()};
+
+  m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
+                            SwerveModuleState(), SwerveModuleState(),
+                            SwerveModuleState());
+  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
+                                        state, state);
+
+  EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}

wpimath/src/test/native/cpp/main.cpp

diff --git a/wpimath/src/test/native/cpp/main.cpp b/wpimath/src/test/native/cpp/main.cpp
new file mode 100644
index 0000000..e2126f2
--- /dev/null
+++ b/wpimath/src/test/native/cpp/main.cpp
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp

diff --git a/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
new file mode 100644
index 0000000..fe084e0
--- /dev/null
+++ b/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+#include <vector>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+#include "units/length.h"
+
+using namespace frc;
+
+namespace frc {
+class CubicHermiteSplineTest : public ::testing::Test {
+ protected:
+  static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
+                  const Pose2d& b) {
+    // Start the timer.
+    const auto start = std::chrono::high_resolution_clock::now();
+
+    // Generate and parameterize the spline.
+
+    const auto [startCV, endCV] =
+        SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b);
+
+    const auto splines =
+        SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
+    std::vector<Spline<3>::PoseWithCurvature> poses;
+
+    poses.push_back(splines[0].GetPoint(0.0));
+
+    for (auto&& spline : splines) {
+      auto x = SplineParameterizer::Parameterize(spline);
+      poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
+    }
+
+    // End timer.
+    const auto finish = std::chrono::high_resolution_clock::now();
+
+    // Calculate the duration (used when benchmarking)
+    const auto duration =
+        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+    for (unsigned int i = 0; i < poses.size() - 1; i++) {
+      auto& p0 = poses[i];
+      auto& p1 = poses[i + 1];
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      auto twist = p0.first.Log(p1.first);
+      EXPECT_LT(std::abs(twist.dx.to<double>()),
+                SplineParameterizer::kMaxDx.to<double>());
+      EXPECT_LT(std::abs(twist.dy.to<double>()),
+                SplineParameterizer::kMaxDy.to<double>());
+      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+                SplineParameterizer::kMaxDtheta.to<double>());
+    }
+
+    // Check first point.
+    EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+                a.Rotation().Radians().to<double>(), 1E-9);
+
+    // Check interior waypoints
+    bool interiorsGood = true;
+    for (auto& waypoint : waypoints) {
+      bool found = false;
+      for (auto& state : poses) {
+        if (std::abs(
+                waypoint.Distance(state.first.Translation()).to<double>()) <
+            1E-9) {
+          found = true;
+        }
+      }
+      interiorsGood &= found;
+    }
+
+    EXPECT_TRUE(interiorsGood);
+
+    // Check last point.
+    EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+                b.Rotation().Radians().to<double>(), 1E-9);
+
+    static_cast<void>(duration);
+  }
+};
+}  // namespace frc
+
+TEST_F(CubicHermiteSplineTest, StraightLine) {
+  Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(CubicHermiteSplineTest, SCurve) {
+  Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
+  std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
+                                       Translation2d(2_m, -1_m)};
+  Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
+  Run(start, waypoints, end);
+}
+
+TEST_F(CubicHermiteSplineTest, OneInterior) {
+  Pose2d start{0_m, 0_m, 0_rad};
+  std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
+  Pose2d end{4_m, 0_m, 0_rad};
+  Run(start, waypoints, end);
+}
+
+TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
+  EXPECT_THROW(
+      Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
+          Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
+      SplineParameterizer::MalformedSplineException);
+  EXPECT_THROW(
+      Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
+          Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
+      SplineParameterizer::MalformedSplineException);
+}

wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp

diff --git a/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
new file mode 100644
index 0000000..30b5b31
--- /dev/null
+++ b/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/length.h"
+
+using namespace frc;
+
+namespace frc {
+class QuinticHermiteSplineTest : public ::testing::Test {
+ protected:
+  static void Run(const Pose2d& a, const Pose2d& b) {
+    // Start the timer.
+    const auto start = std::chrono::high_resolution_clock::now();
+
+    // Generate and parameterize the spline.
+    const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0];
+    const auto poses = SplineParameterizer::Parameterize(spline);
+
+    // End timer.
+    const auto finish = std::chrono::high_resolution_clock::now();
+
+    // Calculate the duration (used when benchmarking)
+    const auto duration =
+        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+    for (unsigned int i = 0; i < poses.size() - 1; i++) {
+      auto& p0 = poses[i];
+      auto& p1 = poses[i + 1];
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      auto twist = p0.first.Log(p1.first);
+      EXPECT_LT(std::abs(twist.dx.to<double>()),
+                SplineParameterizer::kMaxDx.to<double>());
+      EXPECT_LT(std::abs(twist.dy.to<double>()),
+                SplineParameterizer::kMaxDy.to<double>());
+      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+                SplineParameterizer::kMaxDtheta.to<double>());
+    }
+
+    // Check first point.
+    EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+                a.Rotation().Radians().to<double>(), 1E-9);
+
+    // Check last point.
+    EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+                b.Rotation().Radians().to<double>(), 1E-9);
+
+    static_cast<void>(duration);
+  }
+};
+}  // namespace frc
+
+TEST_F(QuinticHermiteSplineTest, StraightLine) {
+  Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
+  Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
+  Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
+      Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
+}
+
+TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
+  EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+                   Pose2d(1_m, 0_m, Rotation2d(180_deg))),
+               SplineParameterizer::MalformedSplineException);
+  EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
+                   Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
+               SplineParameterizer::MalformedSplineException);
+}

wpimath/src/test/native/cpp/system/DiscretizationTest.cpp

diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
new file mode 100644
index 0000000..dbeb518
--- /dev/null
+++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -0,0 +1,245 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <functional>
+
+#include "Eigen/Core"
+#include "Eigen/Eigenvalues"
+#include "frc/system/Discretization.h"
+#include "frc/system/RungeKutta.h"
+
+// Check that for a simple second-order system that we can easily analyze
+// analytically,
+TEST(DiscretizationTest, DiscretizeA) {
+  Eigen::Matrix<double, 2, 2> contA;
+  contA << 0, 1, 0, 0;
+
+  Eigen::Matrix<double, 2, 1> x0;
+  x0 << 1, 1;
+  Eigen::Matrix<double, 2, 2> discA;
+
+  frc::DiscretizeA<2>(contA, 1_s, &discA);
+  Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0;
+
+  // We now have pos = vel = 1 and accel = 0, which should give us:
+  Eigen::Matrix<double, 2, 1> x1Truth;
+  x1Truth(1) = x0(1);
+  x1Truth(0) = x0(0) + 1.0 * x0(1);
+
+  EXPECT_EQ(x1Truth, x1Discrete);
+}
+
+// Check that for a simple second-order system that we can easily analyze
+// analytically,
+TEST(DiscretizationTest, DiscretizeAB) {
+  Eigen::Matrix<double, 2, 2> contA;
+  contA << 0, 1, 0, 0;
+
+  Eigen::Matrix<double, 2, 1> contB;
+  contB << 0, 1;
+
+  Eigen::Matrix<double, 2, 1> x0;
+  x0 << 1, 1;
+  Eigen::Matrix<double, 1, 1> u;
+  u << 1;
+  Eigen::Matrix<double, 2, 2> discA;
+  Eigen::Matrix<double, 2, 1> discB;
+
+  frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
+  Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0 + discB * u;
+
+  // We now have pos = vel = accel = 1, which should give us:
+  Eigen::Matrix<double, 2, 1> x1Truth;
+  x1Truth(1) = x0(1) + 1.0 * u(0);
+  x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0);
+
+  EXPECT_EQ(x1Truth, x1Discrete);
+}
+
+// Test that the discrete approximation of Q is roughly equal to
+// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
+  Eigen::Matrix<double, 2, 2> contA;
+  contA << 0, 1, 0, 0;
+
+  Eigen::Matrix<double, 2, 2> contQ;
+  contQ << 1, 0, 0, 1;
+
+  constexpr auto dt = 1_s;
+
+  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+      std::function<Eigen::Matrix<double, 2, 2>(
+          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
+      Eigen::Matrix<double, 2, 2>>(
+      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
+        return Eigen::Matrix<double, 2, 2>(
+            (contA * t.to<double>()).exp() * contQ *
+            (contA.transpose() * t.to<double>()).exp());
+      },
+      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+
+  Eigen::Matrix<double, 2, 2> discA;
+  Eigen::Matrix<double, 2, 2> discQ;
+  frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
+
+  EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10)
+      << "Expected these to be nearly equal:\ndiscQ:\n"
+      << discQ << "\ndiscQIntegrated:\n"
+      << discQIntegrated;
+}
+
+// Test that the discrete approximation of Q is roughly equal to
+// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+TEST(DiscretizationTest, DiscretizeFastModelAQ) {
+  Eigen::Matrix<double, 2, 2> contA;
+  contA << 0, 1, 0, -1406.29;
+
+  Eigen::Matrix<double, 2, 2> contQ;
+  contQ << 0.0025, 0, 0, 1;
+
+  constexpr auto dt = 5.05_ms;
+
+  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+      std::function<Eigen::Matrix<double, 2, 2>(
+          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
+      Eigen::Matrix<double, 2, 2>>(
+      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
+        return Eigen::Matrix<double, 2, 2>(
+            (contA * t.to<double>()).exp() * contQ *
+            (contA.transpose() * t.to<double>()).exp());
+      },
+      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+
+  Eigen::Matrix<double, 2, 2> discA;
+  Eigen::Matrix<double, 2, 2> discQ;
+  frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
+
+  EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3)
+      << "Expected these to be nearly equal:\ndiscQ:\n"
+      << discQ << "\ndiscQIntegrated:\n"
+      << discQIntegrated;
+}
+
+// Test that the Taylor series discretization produces nearly identical results.
+TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
+  Eigen::Matrix<double, 2, 2> contA;
+  contA << 0, 1, 0, 0;
+
+  Eigen::Matrix<double, 2, 1> contB;
+  contB << 0, 1;
+
+  Eigen::Matrix<double, 2, 2> contQ;
+  contQ << 1, 0, 0, 1;
+
+  constexpr auto dt = 1_s;
+
+  Eigen::Matrix<double, 2, 2> discQTaylor;
+  Eigen::Matrix<double, 2, 2> discA;
+  Eigen::Matrix<double, 2, 2> discATaylor;
+  Eigen::Matrix<double, 2, 1> discB;
+
+  // Continuous Q should be positive semidefinite
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
+  for (int i = 0; i < contQ.rows(); i++) {
+    EXPECT_GT(esCont.eigenvalues()[i], 0);
+  }
+
+  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+      std::function<Eigen::Matrix<double, 2, 2>(
+          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
+      Eigen::Matrix<double, 2, 2>>(
+      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
+        return Eigen::Matrix<double, 2, 2>(
+            (contA * t.to<double>()).exp() * contQ *
+            (contA.transpose() * t.to<double>()).exp());
+      },
+      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+
+  frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+  frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
+
+  EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
+      << "Expected these to be nearly equal:\ndiscQTaylor:\n"
+      << discQTaylor << "\ndiscQIntegrated:\n"
+      << discQIntegrated;
+  EXPECT_LT((discA - discATaylor).norm(), 1e-10);
+
+  // Discrete Q should be positive semidefinite
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
+  for (int i = 0; i < discQTaylor.rows(); i++) {
+    EXPECT_GT(esDisc.eigenvalues()[i], 0);
+  }
+}
+
+// Test that the Taylor series discretization produces nearly identical results.
+TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
+  Eigen::Matrix<double, 2, 2> contA;
+  contA << 0, 1, 0, -1500;
+
+  Eigen::Matrix<double, 2, 1> contB;
+  contB << 0, 1;
+
+  Eigen::Matrix<double, 2, 2> contQ;
+  contQ << 0.0025, 0, 0, 1;
+
+  constexpr auto dt = 5.05_ms;
+
+  Eigen::Matrix<double, 2, 2> discQTaylor;
+  Eigen::Matrix<double, 2, 2> discA;
+  Eigen::Matrix<double, 2, 2> discATaylor;
+  Eigen::Matrix<double, 2, 1> discB;
+
+  // Continuous Q should be positive semidefinite
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
+  for (int i = 0; i < contQ.rows(); i++) {
+    EXPECT_GT(esCont.eigenvalues()[i], 0);
+  }
+
+  Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
+      std::function<Eigen::Matrix<double, 2, 2>(
+          units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
+      Eigen::Matrix<double, 2, 2>>(
+      [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
+        return Eigen::Matrix<double, 2, 2>(
+            (contA * t.to<double>()).exp() * contQ *
+            (contA.transpose() * t.to<double>()).exp());
+      },
+      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+
+  frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+  frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
+
+  EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
+      << "Expected these to be nearly equal:\ndiscQTaylor:\n"
+      << discQTaylor << "\ndiscQIntegrated:\n"
+      << discQIntegrated;
+  EXPECT_LT((discA - discATaylor).norm(), 1e-10);
+
+  // Discrete Q should be positive semidefinite
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
+  for (int i = 0; i < discQTaylor.rows(); i++) {
+    EXPECT_GT(esDisc.eigenvalues()[i], 0);
+  }
+}
+
+// Test that DiscretizeR() works
+TEST(DiscretizationTest, DiscretizeR) {
+  Eigen::Matrix<double, 2, 2> contR;
+  contR << 2.0, 0.0, 0.0, 1.0;
+
+  Eigen::Matrix<double, 2, 2> discRTruth;
+  discRTruth << 4.0, 0.0, 0.0, 2.0;
+
+  Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
+
+  EXPECT_LT((discRTruth - discR).norm(), 1e-10)
+      << "Expected these to be nearly equal:\ndiscR:\n"
+      << discR << "\ndiscRTruth:\n"
+      << discRTruth;
+}

wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp

diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
new file mode 100644
index 0000000..eedf8c0
--- /dev/null
+++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <gtest/gtest.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/length.h"
+#include "units/mass.h"
+
+TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
+  auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
+      frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
+
+  ASSERT_TRUE(model.A().isApprox(
+      frc::MakeMatrix<2, 2>(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
+  ASSERT_TRUE(model.B().isApprox(
+      frc::MakeMatrix<2, 2>(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
+  ASSERT_TRUE(
+      model.C().isApprox(frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0), 0.001));
+  ASSERT_TRUE(
+      model.D().isApprox(frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0), 0.001));
+}
+
+TEST(LinearSystemIDTest, ElevatorSystem) {
+  auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
+                                                   0.05_m, 12);
+  ASSERT_TRUE(model.A().isApprox(
+      frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -99.05473), 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 20.8), 0.001));
+  ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 2>(1.0, 0.0), 0.001));
+  ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+}
+
+TEST(LinearSystemIDTest, FlywheelSystem) {
+  auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
+                                                   0.00032_kg_sq_m, 1.0);
+  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-26.87032), 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1354.166667), 0.001));
+  ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 1>(1.0), 0.001));
+  ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+}
+
+TEST(LinearSystemIDTest, IdentifyPositionSystem) {
+  // By controls engineering in frc,
+  // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
+  double kv = 1.0;
+  double ka = 0.5;
+  auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
+      kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
+
+  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka),
+                                 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 1.0 / ka), 0.001));
+}
+
+TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
+  // By controls engineering in frc,
+  // V = kv * velocity + ka * acceleration
+  // x-dot =  -kv/ka * v + 1/ka \cdot V
+  double kv = 1.0;
+  double ka = 0.5;
+  auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
+      kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
+
+  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001));
+  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001));
+}

wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp

diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
new file mode 100644
index 0000000..ddc3f68
--- /dev/null
+++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include "frc/system/NumericalJacobian.h"
+
+Eigen::Matrix<double, 4, 4> A = (Eigen::Matrix<double, 4, 4>() << 1, 2, 4, 1, 5,
+                                 2, 3, 4, 5, 1, 3, 2, 1, 1, 3, 7)
+                                    .finished();
+
+Eigen::Matrix<double, 4, 2> B =
+    (Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
+
+// Function from which to recover A and B
+Eigen::Matrix<double, 4, 1> AxBuFn(const Eigen::Matrix<double, 4, 1>& x,
+                                   const Eigen::Matrix<double, 2, 1>& u) {
+  return A * x + B * u;
+}
+
+// Test that we can recover A from AxBuFn() pretty accurately
+TEST(NumericalJacobianTest, Ax) {
+  Eigen::Matrix<double, 4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
+      AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
+      Eigen::Matrix<double, 2, 1>::Zero());
+  EXPECT_TRUE(newA.isApprox(A));
+}
+
+// Test that we can recover B from AxBuFn() pretty accurately
+TEST(NumericalJacobianTest, Bu) {
+  Eigen::Matrix<double, 4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
+      AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
+      Eigen::Matrix<double, 2, 1>::Zero());
+  EXPECT_TRUE(newB.isApprox(B));
+}
+
+Eigen::Matrix<double, 3, 4> C =
+    (Eigen::Matrix<double, 3, 4>() << 1, 2, 4, 1, 5, 2, 3, 4, 5, 1, 3, 2)
+        .finished();
+
+Eigen::Matrix<double, 3, 2> D =
+    (Eigen::Matrix<double, 3, 2>() << 1, 1, 2, 1, 3, 2).finished();
+
+// Function from which to recover C and D
+Eigen::Matrix<double, 3, 1> CxDuFn(const Eigen::Matrix<double, 4, 1>& x,
+                                   const Eigen::Matrix<double, 2, 1>& u) {
+  return C * x + D * u;
+}
+
+// Test that we can recover C from CxDuFn() pretty accurately
+TEST(NumericalJacobianTest, Cx) {
+  Eigen::Matrix<double, 3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
+      CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
+      Eigen::Matrix<double, 2, 1>::Zero());
+  EXPECT_TRUE(newC.isApprox(C));
+}
+
+// Test that we can recover D from CxDuFn() pretty accurately
+TEST(NumericalJacobianTest, Du) {
+  Eigen::Matrix<double, 3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
+      CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
+      Eigen::Matrix<double, 2, 1>::Zero());
+  EXPECT_TRUE(newD.isApprox(D));
+}

wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp

diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
new file mode 100644
index 0000000..a12c1b7
--- /dev/null
+++ b/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "frc/system/RungeKutta.h"
+
+// Tests that integrating dx/dt = e^x works.
+TEST(RungeKuttaTest, Exponential) {
+  Eigen::Matrix<double, 1, 1> y0;
+  y0(0) = 0.0;
+
+  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
+      [](Eigen::Matrix<double, 1, 1> x) {
+        Eigen::Matrix<double, 1, 1> y;
+        y(0) = std::exp(x(0));
+        return y;
+      },
+      y0, 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests that integrating dx/dt = e^x works when we provide a U.
+TEST(RungeKuttaTest, ExponentialWithU) {
+  Eigen::Matrix<double, 1, 1> y0;
+  y0(0) = 0.0;
+
+  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
+      [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
+        Eigen::Matrix<double, 1, 1> y;
+        y(0) = std::exp(u(0) * x(0));
+        return y;
+      },
+      y0, (Eigen::Matrix<double, 1, 1>() << 1.0).finished(), 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+namespace {
+Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
+  return (Eigen::Matrix<double, 1, 1>()
+          << 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
+      .finished();
+}
+}  // namespace
+
+// Tests RungeKutta with a time varying solution.
+// Now, lets test RK4 with a time varying solution.  From
+// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+//   x' = x (2 / (e^t + 1) - 1)
+//
+// The true (analytical) solution is:
+//
+// x(t) = 12 * e^t / ((e^t + 1)^2)
+TEST(RungeKuttaTest, RungeKuttaTimeVarying) {
+  Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
+      [](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
+        return (Eigen::Matrix<double, 1, 1>()
+                << x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
+            .finished();
+      },
+      y0, 5_s, 1_s);
+  EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
+}

wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
new file mode 100644
index 0000000..42e9fe2
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+#include "units/acceleration.h"
+#include "units/angle.h"
+#include "units/math.h"
+#include "units/velocity.h"
+
+using namespace frc;
+
+TEST(CentripetalAccelerationConstraintTest, Constraint) {
+  const auto maxCentripetalAcceleration = 7_fps_sq;
+
+  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+  config.AddConstraint(
+      CentripetalAccelerationConstraint(maxCentripetalAcceleration));
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    auto centripetalAcceleration =
+        units::math::pow<2>(point.velocity) * point.curvature / 1_rad;
+
+    EXPECT_TRUE(centripetalAcceleration <
+                maxCentripetalAcceleration + 0.05_mps_sq);
+  }
+}

wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
new file mode 100644
index 0000000..636b002
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+#include "units/time.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
+  const auto maxVelocity = 12_fps;
+  const DifferentialDriveKinematics kinematics{27_in};
+
+  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+  config.AddConstraint(
+      DifferentialDriveKinematicsConstraint(kinematics, maxVelocity));
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+                                      point.velocity * point.curvature};
+
+    auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+    EXPECT_TRUE(left < maxVelocity + 0.05_mps);
+    EXPECT_TRUE(right < maxVelocity + 0.05_mps);
+  }
+}

wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
new file mode 100644
index 0000000..6cd8075
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+#include <memory>
+#include <vector>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
+  // Pick an unreasonably large kA to ensure the constraint has to do some work
+  SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
+                                                   3_V / 1_mps_sq};
+  const DifferentialDriveKinematics kinematics{0.5_m};
+  const auto maxVoltage = 10_V;
+
+  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+  config.AddConstraint(
+      DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+                                      point.velocity * point.curvature};
+
+    auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+    // Not really a strictly-correct test as we're using the chassis accel
+    // instead of the wheel accel, but much easier than doing it "properly" and
+    // a reasonable check anyway
+    EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
+                maxVoltage + 0.05_V);
+    EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
+                -maxVoltage - 0.05_V);
+    EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
+                maxVoltage + 0.05_V);
+    EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
+                -maxVoltage - 0.05_V);
+  }
+}
+
+TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) {
+  SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
+                                                   3_V / 1_mps_sq};
+  // Large trackwidth - need to test with radius of curvature less than half of
+  // trackwidth
+  const DifferentialDriveKinematics kinematics{3_m};
+  const auto maxVoltage = 10_V;
+
+  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+  config.AddConstraint(
+      DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
+
+  EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
+      Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector<Translation2d>{},
+      Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config));
+
+  config.SetReversed(true);
+
+  EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
+      Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector<Translation2d>{},
+      Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config));
+}

wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
new file mode 100644
index 0000000..44c4222
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
+#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+#include "units/acceleration.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/velocity.h"
+
+using namespace frc;
+
+TEST(EllipticalRegionConstraintTest, Constraint) {
+  constexpr auto maxVelocity = 2_fps;
+
+  auto config = TrajectoryConfig(13_fps, 13_fps_sq);
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  EllipticalRegionConstraint regionConstraint(frc::Translation2d{5_ft, 2.5_ft},
+                                              10_ft, 5_ft, Rotation2d(180_deg),
+                                              maxVelConstraint);
+  config.AddConstraint(regionConstraint);
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  bool exceededConstraintOutsideRegion = false;
+  for (auto& point : trajectory.States()) {
+    auto translation = point.pose.Translation();
+    if (translation.X() < 10_ft && translation.Y() < 5_ft) {
+      EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
+    } else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
+      exceededConstraintOutsideRegion = true;
+    }
+  }
+
+  EXPECT_TRUE(exceededConstraintOutsideRegion);
+}
+
+TEST(EllipticalRegionConstraintTest, IsPoseInRegion) {
+  constexpr auto maxVelocity = 2_fps;
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  EllipticalRegionConstraint regionConstraintNoRotation(
+      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(0_deg),
+      maxVelConstraint);
+  EXPECT_FALSE(regionConstraintNoRotation.IsPoseInRegion(
+      frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+
+  EllipticalRegionConstraint regionConstraintWithRotation(
+      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(90_deg),
+      maxVelConstraint);
+  EXPECT_TRUE(regionConstraintWithRotation.IsPoseInRegion(
+      frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+}

wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
new file mode 100644
index 0000000..33f753f
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
+#include "frc/trajectory/constraint/RectangularRegionConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/math.h"
+#include "units/velocity.h"
+
+using namespace frc;
+
+TEST(RectangularRegionConstraintTest, Constraint) {
+  constexpr auto maxVelocity = 2_fps;
+
+  auto config = TrajectoryConfig(13_fps, 13_fps_sq);
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft},
+                                               frc::Translation2d{5_ft, 27_ft},
+                                               maxVelConstraint);
+  config.AddConstraint(regionConstraint);
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  bool exceededConstraintOutsideRegion = false;
+  for (auto& point : trajectory.States()) {
+    if (regionConstraint.IsPoseInRegion(point.pose)) {
+      EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
+    } else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
+      exceededConstraintOutsideRegion = true;
+    }
+  }
+
+  EXPECT_TRUE(exceededConstraintOutsideRegion);
+}
+
+TEST(RectangularRegionConstraintTest, IsPoseInRegion) {
+  constexpr auto maxVelocity = 2_fps;
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft},
+                                               frc::Translation2d{5_ft, 27_ft},
+                                               maxVelConstraint);
+
+  EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d()));
+  EXPECT_TRUE(
+      regionConstraint.IsPoseInRegion(Pose2d(3_ft, 14.5_ft, Rotation2d())));
+}

wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
new file mode 100644
index 0000000..378aff7
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+#include "units/math.h"
+
+using namespace frc;
+
+TEST(TrajectoryGenerationTest, ObeysConstraints) {
+  TrajectoryConfig config{12_fps, 12_fps_sq};
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps);
+    EXPECT_TRUE(units::math::abs(point.acceleration) <=
+                12_fps_sq + 0.01_fps_sq);
+  }
+}
+
+TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
+  const auto t = TrajectoryGenerator::GenerateTrajectory(
+      std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+                          Pose2d(1_m, 0_m, Rotation2d(180_deg))},
+      TrajectoryConfig(12_fps, 12_fps_sq));
+
+  ASSERT_EQ(t.States().size(), 1u);
+  ASSERT_EQ(t.TotalTime(), 0_s);
+}

wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
new file mode 100644
index 0000000..c18a3e9
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(TrajectoryJsonTest, DeserializeMatches) {
+  TrajectoryConfig config{12_fps, 12_fps_sq};
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  Trajectory deserialized;
+  EXPECT_NO_THROW(deserialized = TrajectoryUtil::DeserializeTrajectory(
+                      TrajectoryUtil::SerializeTrajectory(trajectory)));
+  EXPECT_EQ(trajectory.States(), deserialized.States());
+}

wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
new file mode 100644
index 0000000..349ff5c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
+                              std::vector<frc::Trajectory::State> statesB) {
+  for (unsigned int i = 0; i < statesA.size() - 1; i++) {
+    auto a1 = statesA[i].pose;
+    auto a2 = statesA[i + 1].pose;
+
+    auto b1 = statesB[i].pose;
+    auto b2 = statesB[i + 1].pose;
+
+    auto a = a2.RelativeTo(a1);
+    auto b = b2.RelativeTo(b1);
+
+    EXPECT_NEAR(a.X().to<double>(), b.X().to<double>(), 1E-9);
+    EXPECT_NEAR(a.Y().to<double>(), b.Y().to<double>(), 1E-9);
+    EXPECT_NEAR(a.Rotation().Radians().to<double>(),
+                b.Rotation().Radians().to<double>(), 1E-9);
+  }
+}
+
+TEST(TrajectoryTransforms, TransformBy) {
+  frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
+      config);
+
+  auto transformedTrajectory =
+      trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
+
+  auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+  EXPECT_NEAR(firstPose.X().to<double>(), 1.0, 1E-9);
+  EXPECT_NEAR(firstPose.Y().to<double>(), 2.0, 1E-9);
+  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
+
+  TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}
+
+TEST(TrajectoryTransforms, RelativeTo) {
+  frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
+      frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
+
+  auto transformedTrajectory =
+      trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
+
+  auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+  EXPECT_NEAR(firstPose.X().to<double>(), 0, 1E-9);
+  EXPECT_NEAR(firstPose.Y().to<double>(), 0, 1E-9);
+  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
+
+  TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}

wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp

diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
new file mode 100644
index 0000000..63ea916
--- /dev/null
+++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -0,0 +1,237 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrapezoidProfile.h"  // NOLINT(build/include_order)
+
+#include <chrono>
+#include <cmath>
+
+#include "gtest/gtest.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/math.h"
+#include "units/velocity.h"
+
+static constexpr auto kDt = 10_ms;
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
+  if (val1 <= val2) {                            \
+    EXPECT_LE(val1, val2);                       \
+  } else {                                       \
+    EXPECT_NEAR_UNITS(val1, val2, eps);          \
+  }
+
+TEST(TrapezoidProfileTest, ReachesGoal) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
+
+  for (int i = 0; i < 450; ++i) {
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
+
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto lastPos = state.position;
+  for (int i = 0; i < 1600; ++i) {
+    if (i == 400) {
+      constraints.maxVelocity = 0.75_mps;
+    }
+
+    profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
+    state = profile.Calculate(kDt);
+    auto estimatedVel = (state.position - lastPos) / kDt;
+
+    if (i >= 400) {
+      // Since estimatedVel can have floating point rounding errors, we check
+      // whether value is less than or within an error delta of the new
+      // constraint.
+      EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
+
+      EXPECT_LE(state.velocity, constraints.maxVelocity);
+    }
+
+    lastPos = state.position;
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// There is some somewhat tricky code for dealing with going backwards
+TEST(TrapezoidProfileTest, Backwards) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
+
+  for (int i = 0; i < 400; ++i) {
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
+
+  for (int i = 0; i < 200; ++i) {
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_NE(state, goal);
+
+  goal = {0.0_m, 0.0_mps};
+  for (int i = 0; i < 550; ++i) {
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed
+TEST(TrapezoidProfileTest, TopSpeed) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
+
+  for (int i = 0; i < 200; ++i) {
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
+
+  for (int i = 0; i < 2000; ++i) {
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, TimingToCurrent) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
+
+  for (int i = 0; i < 400; i++) {
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+    EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingToGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal && state == goal) {
+      // Expected value using for loop index is just an approximation since the
+      // time left in the profile doesn't increase linearly at the endpoints
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+      reachedGoal = true;
+    }
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal &&
+        (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+      reachedGoal = true;
+    }
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal && state == goal) {
+      // Expected value using for loop index is just an approximation since the
+      // time left in the profile doesn't increase linearly at the endpoints
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+      reachedGoal = true;
+    }
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal &&
+        (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+      reachedGoal = true;
+    }
+  }
+}

wpimath/src/test/native/include/drake/common/autodiff.h

diff --git a/wpimath/src/test/native/include/drake/common/autodiff.h b/wpimath/src/test/native/include/drake/common/autodiff.h
new file mode 100644
index 0000000..66fd88a
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/autodiff.h
@@ -0,0 +1,34 @@
+#pragma once
+/// @file This header provides a single inclusion point for autodiff-related
+/// header files in the `drake/common` directory. Users should include this
+/// file. Including other individual headers such as `drake/common/autodiffxd.h`
+/// will generate a compile-time warning.
+
+// In each header included below, it asserts that this macro
+// `DRAKE_COMMON_AUTODIFF_HEADER` is defined. If the macro is not defined, it
+// generates diagnostic warning messages.
+#define DRAKE_COMMON_AUTODIFF_HEADER
+
+#include <Eigen/Core>
+#include <unsupported/Eigen/AutoDiff>
+
+static_assert(EIGEN_VERSION_AT_LEAST(3, 3, 3),
+              "Drake requires Eigen >= v3.3.3.");
+
+// Do not alpha-sort the following block of hard-coded #includes, which is
+// protected by `clang-format on/off`.
+//
+// Rationale: We want to maximize the use of this header, `autodiff.h`, even
+// inside of the autodiff-related files to avoid any mistakes which might not be
+// detected. By centralizing the list here, we make sure that everyone will see
+// the correct order which respects the inter-dependencies of the autodiff
+// headers. This shields us from triggering undefined behaviors due to
+// order-of-specialization-includes-changed mistakes.
+//
+// clang-format off
+#include "drake/common/eigen_autodiff_limits.h"
+#include "drake/common/eigen_autodiff_types.h"
+#include "drake/common/autodiffxd.h"
+#include "drake/common/autodiff_overloads.h"
+// clang-format on
+#undef DRAKE_COMMON_AUTODIFF_HEADER

wpimath/src/test/native/include/drake/common/autodiff_overloads.h

diff --git a/wpimath/src/test/native/include/drake/common/autodiff_overloads.h b/wpimath/src/test/native/include/drake/common/autodiff_overloads.h
new file mode 100644
index 0000000..7eaeb3f
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/autodiff_overloads.h
@@ -0,0 +1,203 @@
+/// @file
+/// Overloads for STL mathematical operations on AutoDiffScalar.
+///
+/// Used via argument-dependent lookup (ADL). These functions appear
+/// in the Eigen namespace so that ADL can automatically choose between
+/// the STL version and the overloaded version to match the type of the
+/// arguments. The proper use would be e.g.
+///
+/// \code{.cc}
+///    void mymethod() {
+///       using std::isinf;
+///       isinf(myval);
+///    }
+/// \endcode{}
+///
+/// @note The if_then_else and cond functions for AutoDiffScalar are in
+/// namespace drake because cond is defined in namespace drake in
+/// "drake/common/cond.h" file.
+
+#pragma once
+
+#ifndef DRAKE_COMMON_AUTODIFF_HEADER
+// TODO(soonho-tri): Change to #error.
+#warning Do not directly include this file. Include "drake/common/autodiff.h".
+#endif
+
+#include <cmath>
+#include <limits>
+
+#include "drake/common/cond.h"
+#include "drake/common/drake_assert.h"
+#include "drake/common/dummy_value.h"
+
+namespace Eigen {
+
+/// Overloads nexttoward to mimic std::nexttoward from <cmath>.
+template <typename DerType>
+double nexttoward(const Eigen::AutoDiffScalar<DerType>& from, long double to) {
+  using std::nexttoward;
+  return nexttoward(from.value(), to);
+}
+
+/// Overloads round to mimic std::round from <cmath>.
+template <typename DerType>
+double round(const Eigen::AutoDiffScalar<DerType>& x) {
+  using std::round;
+  return round(x.value());
+}
+
+/// Overloads isinf to mimic std::isinf from <cmath>.
+template <typename DerType>
+bool isinf(const Eigen::AutoDiffScalar<DerType>& x) {
+  using std::isinf;
+  return isinf(x.value());
+}
+
+/// Overloads isfinite to mimic std::isfinite from <cmath>.
+template <typename DerType>
+bool isfinite(const Eigen::AutoDiffScalar<DerType>& x) {
+  using std::isfinite;
+  return isfinite(x.value());
+}
+
+/// Overloads isnan to mimic std::isnan from <cmath>.
+template <typename DerType>
+bool isnan(const Eigen::AutoDiffScalar<DerType>& x) {
+  using std::isnan;
+  return isnan(x.value());
+}
+
+/// Overloads floor to mimic std::floor from <cmath>.
+template <typename DerType>
+double floor(const Eigen::AutoDiffScalar<DerType>& x) {
+  using std::floor;
+  return floor(x.value());
+}
+
+/// Overloads ceil to mimic std::ceil from <cmath>.
+template <typename DerType>
+double ceil(const Eigen::AutoDiffScalar<DerType>& x) {
+  using std::ceil;
+  return ceil(x.value());
+}
+
+/// Overloads copysign from <cmath>.
+template <typename DerType, typename T>
+Eigen::AutoDiffScalar<DerType> copysign(const Eigen::AutoDiffScalar<DerType>& x,
+                                        const T& y) {
+  using std::isnan;
+  if (isnan(x)) return (y >= 0) ? NAN : -NAN;
+  if ((x < 0 && y >= 0) || (x >= 0 && y < 0))
+    return -x;
+  else
+    return x;
+}
+
+/// Overloads copysign from <cmath>.
+template <typename DerType>
+double copysign(double x, const Eigen::AutoDiffScalar<DerType>& y) {
+  using std::isnan;
+  if (isnan(x)) return (y >= 0) ? NAN : -NAN;
+  if ((x < 0 && y >= 0) || (x >= 0 && y < 0))
+    return -x;
+  else
+    return x;
+}
+
+/// Overloads pow for an AutoDiffScalar base and exponent, implementing the
+/// chain rule.
+template <typename DerTypeA, typename DerTypeB>
+Eigen::AutoDiffScalar<
+    typename internal::remove_all<DerTypeA>::type::PlainObject>
+pow(const Eigen::AutoDiffScalar<DerTypeA>& base,
+    const Eigen::AutoDiffScalar<DerTypeB>& exponent) {
+  // The two AutoDiffScalars being exponentiated must have the same matrix
+  // type. This includes, but is not limited to, the same scalar type and
+  // the same dimension.
+  static_assert(
+      std::is_same<
+          typename internal::remove_all<DerTypeA>::type::PlainObject,
+          typename internal::remove_all<DerTypeB>::type::PlainObject>::value,
+      "The derivative types must match.");
+
+  internal::make_coherent(base.derivatives(), exponent.derivatives());
+
+  const auto& x = base.value();
+  const auto& xgrad = base.derivatives();
+  const auto& y = exponent.value();
+  const auto& ygrad = exponent.derivatives();
+
+  using std::pow;
+  using std::log;
+  const auto x_to_the_y = pow(x, y);
+  if (ygrad.isZero(std::numeric_limits<double>::epsilon()) ||
+      ygrad.size() == 0) {
+    // The derivative only depends on ∂(x^y)/∂x -- this prevents undefined
+    // behavior in the corner case where ∂(x^y)/∂y is infinite when x = 0,
+    // despite ∂y/∂v being 0.
+    return Eigen::MakeAutoDiffScalar(x_to_the_y, y * pow(x, y - 1) * xgrad);
+  }
+  return Eigen::MakeAutoDiffScalar(
+      // The value is x ^ y.
+      x_to_the_y,
+      // The multivariable chain rule states:
+      // df/dv_i = (∂f/∂x * dx/dv_i) + (∂f/∂y * dy/dv_i)
+      // ∂f/∂x is y*x^(y-1)
+      y * pow(x, y - 1) * xgrad +
+      // ∂f/∂y is (x^y)*ln(x)
+      x_to_the_y * log(x) * ygrad);
+}
+
+}  // namespace Eigen
+
+namespace drake {
+
+/// Returns the autodiff scalar's value() as a double.  Never throws.
+/// Overloads ExtractDoubleOrThrow from common/extract_double.h.
+template <typename DerType>
+double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar<DerType>& scalar) {
+  return static_cast<double>(scalar.value());
+}
+
+/// Specializes common/dummy_value.h.
+template <typename DerType>
+struct dummy_value<Eigen::AutoDiffScalar<DerType>> {
+  static constexpr Eigen::AutoDiffScalar<DerType> get() {
+    constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
+    DerType derivatives;
+    derivatives.fill(kNaN);
+    return Eigen::AutoDiffScalar<DerType>(kNaN, derivatives);
+  }
+};
+
+/// Provides if-then-else expression for Eigen::AutoDiffScalar type. To support
+/// Eigen's generic expressions, we use casting to the plain object after
+/// applying Eigen::internal::remove_all. It is based on the Eigen's
+/// implementation of min/max function for AutoDiffScalar type
+/// (https://bitbucket.org/eigen/eigen/src/10a1de58614569c9250df88bdfc6402024687bc6/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h?at=default&fileviewer=file-view-default#AutoDiffScalar.h-546).
+template <typename DerType1, typename DerType2>
+inline Eigen::AutoDiffScalar<
+    typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
+if_then_else(bool f_cond, const Eigen::AutoDiffScalar<DerType1>& x,
+             const Eigen::AutoDiffScalar<DerType2>& y) {
+  typedef Eigen::AutoDiffScalar<
+      typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
+      ADS1;
+  typedef Eigen::AutoDiffScalar<
+      typename Eigen::internal::remove_all<DerType2>::type::PlainObject>
+      ADS2;
+  static_assert(std::is_same<ADS1, ADS2>::value,
+                "The derivative types must match.");
+  return f_cond ? ADS1(x) : ADS2(y);
+}
+
+/// Provides special case of cond expression for Eigen::AutoDiffScalar type.
+template <typename DerType, typename... Rest>
+Eigen::AutoDiffScalar<
+    typename Eigen::internal::remove_all<DerType>::type::PlainObject>
+cond(bool f_cond, const Eigen::AutoDiffScalar<DerType>& e_then, Rest... rest) {
+  return if_then_else(f_cond, e_then, cond(rest...));
+}
+
+}  // namespace drake

wpimath/src/test/native/include/drake/common/autodiffxd.h

diff --git a/wpimath/src/test/native/include/drake/common/autodiffxd.h b/wpimath/src/test/native/include/drake/common/autodiffxd.h
new file mode 100644
index 0000000..a99b2f0
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/autodiffxd.h
@@ -0,0 +1,423 @@
+#pragma once
+
+// This file is a modification of Eigen-3.3.3's AutoDiffScalar.h file which is
+// available at
+// https://bitbucket.org/eigen/eigen/raw/67e894c6cd8f5f1f604b27d37ed47fdf012674ff/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2017 Drake Authors
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef DRAKE_COMMON_AUTODIFF_HEADER
+// TODO(soonho-tri): Change to #error.
+#warning Do not directly include this file. Include "drake/common/autodiff.h".
+#endif
+
+#include <cmath>
+#include <ostream>
+
+#include <Eigen/Core>
+
+namespace Eigen {
+
+#if !defined(DRAKE_DOXYGEN_CXX)
+// Explicit template specializations of Eigen::AutoDiffScalar for VectorXd.
+//
+// AutoDiffScalar tries to call internal::make_coherent to promote empty
+// derivatives. However, it fails to do the promotion when an operand is an
+// expression tree (i.e. CwiseBinaryOp). Our solution is to provide special
+// overloading for VectorXd and change the return types of its operators. With
+// this change, the operators evaluate terms immediately and return an
+// AutoDiffScalar<VectorXd> instead of expression trees (such as CwiseBinaryOp).
+// Eigen's implementation of internal::make_coherent makes use of const_cast in
+// order to promote zero sized derivatives. This however interferes badly with
+// our caching system and produces unexpected behaviors. See #10971 for details.
+// Therefore our implementation stops using internal::make_coherent and treats
+// scalars with zero sized derivatives as constants, as it should.
+//
+// We also provide overloading of math functions for AutoDiffScalar<VectorXd>
+// which return AutoDiffScalar<VectorXd> instead of an expression tree.
+//
+// See https://github.com/RobotLocomotion/drake/issues/6944 for more
+// information. See also drake/common/autodiff_overloads.h.
+//
+// TODO(soonho-tri): Next time when we upgrade Eigen, please check if we still
+// need these specializations.
+template <>
+class AutoDiffScalar<VectorXd>
+    : public internal::auto_diff_special_op<VectorXd, false> {
+ public:
+  typedef internal::auto_diff_special_op<VectorXd, false> Base;
+  typedef typename internal::remove_all<VectorXd>::type DerType;
+  typedef typename internal::traits<DerType>::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real Real;
+
+  using Base::operator+;
+  using Base::operator*;
+
+  AutoDiffScalar() {}
+
+  AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
+      : m_value(value), m_derivatives(DerType::Zero(nbDer)) {
+    m_derivatives.coeffRef(derNumber) = Scalar(1);
+  }
+
+  // NOLINTNEXTLINE(runtime/explicit): Code from Eigen.
+  AutoDiffScalar(const Real& value) : m_value(value) {
+    if (m_derivatives.size() > 0) m_derivatives.setZero();
+  }
+
+  AutoDiffScalar(const Scalar& value, const DerType& der)
+      : m_value(value), m_derivatives(der) {}
+
+  template <typename OtherDerType>
+  AutoDiffScalar(
+      const AutoDiffScalar<OtherDerType>& other
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+      ,
+      typename internal::enable_if<
+          internal::is_same<
+              Scalar, typename internal::traits<typename internal::remove_all<
+                          OtherDerType>::type>::Scalar>::value,
+          void*>::type = 0
+#endif
+      )
+      : m_value(other.value()), m_derivatives(other.derivatives()) {
+  }
+
+  friend std::ostream& operator<<(std::ostream& s, const AutoDiffScalar& a) {
+    return s << a.value();
+  }
+
+  AutoDiffScalar(const AutoDiffScalar& other)
+      : m_value(other.value()), m_derivatives(other.derivatives()) {}
+
+  template <typename OtherDerType>
+  inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other) {
+    m_value = other.value();
+    m_derivatives = other.derivatives();
+    return *this;
+  }
+
+  inline AutoDiffScalar& operator=(const AutoDiffScalar& other) {
+    m_value = other.value();
+    m_derivatives = other.derivatives();
+    return *this;
+  }
+
+  inline AutoDiffScalar& operator=(const Scalar& other) {
+    m_value = other;
+    if (m_derivatives.size() > 0) m_derivatives.setZero();
+    return *this;
+  }
+
+  inline const Scalar& value() const { return m_value; }
+  inline Scalar& value() { return m_value; }
+
+  inline const DerType& derivatives() const { return m_derivatives; }
+  inline DerType& derivatives() { return m_derivatives; }
+
+  inline bool operator<(const Scalar& other) const { return m_value < other; }
+  inline bool operator<=(const Scalar& other) const { return m_value <= other; }
+  inline bool operator>(const Scalar& other) const { return m_value > other; }
+  inline bool operator>=(const Scalar& other) const { return m_value >= other; }
+  inline bool operator==(const Scalar& other) const { return m_value == other; }
+  inline bool operator!=(const Scalar& other) const { return m_value != other; }
+
+  friend inline bool operator<(const Scalar& a, const AutoDiffScalar& b) {
+    return a < b.value();
+  }
+  friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) {
+    return a <= b.value();
+  }
+  friend inline bool operator>(const Scalar& a, const AutoDiffScalar& b) {
+    return a > b.value();
+  }
+  friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) {
+    return a >= b.value();
+  }
+  friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) {
+    return a == b.value();
+  }
+  friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) {
+    return a != b.value();
+  }
+
+  template <typename OtherDerType>
+  inline bool operator<(const AutoDiffScalar<OtherDerType>& b) const {
+    return m_value < b.value();
+  }
+  template <typename OtherDerType>
+  inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const {
+    return m_value <= b.value();
+  }
+  template <typename OtherDerType>
+  inline bool operator>(const AutoDiffScalar<OtherDerType>& b) const {
+    return m_value > b.value();
+  }
+  template <typename OtherDerType>
+  inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const {
+    return m_value >= b.value();
+  }
+  template <typename OtherDerType>
+  inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const {
+    return m_value == b.value();
+  }
+  template <typename OtherDerType>
+  inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const {
+    return m_value != b.value();
+  }
+
+  inline const AutoDiffScalar<DerType> operator+(const Scalar& other) const {
+    return AutoDiffScalar<DerType>(m_value + other, m_derivatives);
+  }
+
+  friend inline const AutoDiffScalar<DerType> operator+(
+      const Scalar& a, const AutoDiffScalar& b) {
+    return AutoDiffScalar<DerType>(a + b.value(), b.derivatives());
+  }
+
+  inline AutoDiffScalar& operator+=(const Scalar& other) {
+    value() += other;
+    return *this;
+  }
+
+  template <typename OtherDerType>
+  inline const AutoDiffScalar<DerType> operator+(
+      const AutoDiffScalar<OtherDerType>& other) const {
+    const bool has_this_der = m_derivatives.size() > 0;
+    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
+    return MakeAutoDiffScalar(
+        m_value + other.value(),
+        has_both_der
+            ? VectorXd(m_derivatives + other.derivatives())
+            : has_this_der ? m_derivatives : VectorXd(other.derivatives()));
+  }
+
+  template <typename OtherDerType>
+  inline AutoDiffScalar& operator+=(const AutoDiffScalar<OtherDerType>& other) {
+    (*this) = (*this) + other;
+    return *this;
+  }
+
+  inline const AutoDiffScalar<DerType> operator-(const Scalar& b) const {
+    return AutoDiffScalar<DerType>(m_value - b, m_derivatives);
+  }
+
+  friend inline const AutoDiffScalar<DerType> operator-(
+      const Scalar& a, const AutoDiffScalar& b) {
+    return AutoDiffScalar<DerType>(a - b.value(), -b.derivatives());
+  }
+
+  inline AutoDiffScalar& operator-=(const Scalar& other) {
+    value() -= other;
+    return *this;
+  }
+
+  template <typename OtherDerType>
+  inline const AutoDiffScalar<DerType> operator-(
+      const AutoDiffScalar<OtherDerType>& other) const {
+    const bool has_this_der = m_derivatives.size() > 0;
+    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
+    return MakeAutoDiffScalar(
+        m_value - other.value(),
+        has_both_der
+            ? VectorXd(m_derivatives - other.derivatives())
+            : has_this_der ? m_derivatives : VectorXd(-other.derivatives()));
+  }
+
+  template <typename OtherDerType>
+  inline AutoDiffScalar& operator-=(const AutoDiffScalar<OtherDerType>& other) {
+    *this = *this - other;
+    return *this;
+  }
+
+  inline const AutoDiffScalar<DerType> operator-() const {
+    return AutoDiffScalar<DerType>(-m_value, -m_derivatives);
+  }
+
+  inline const AutoDiffScalar<DerType> operator*(const Scalar& other) const {
+    return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
+  }
+
+  friend inline const AutoDiffScalar<DerType> operator*(
+      const Scalar& other, const AutoDiffScalar& a) {
+    return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
+  }
+
+  inline const AutoDiffScalar<DerType> operator/(const Scalar& other) const {
+    return MakeAutoDiffScalar(m_value / other,
+                              (m_derivatives * (Scalar(1) / other)));
+  }
+
+  friend inline const AutoDiffScalar<DerType> operator/(
+      const Scalar& other, const AutoDiffScalar& a) {
+    return MakeAutoDiffScalar(
+        other / a.value(),
+        a.derivatives() * (Scalar(-other) / (a.value() * a.value())));
+  }
+
+  template <typename OtherDerType>
+  inline const AutoDiffScalar<DerType> operator/(
+      const AutoDiffScalar<OtherDerType>& other) const {
+    const auto& this_der = m_derivatives;
+    const auto& other_der = other.derivatives();
+    const bool has_this_der = m_derivatives.size() > 0;
+    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
+    const double scale = 1. / (other.value() * other.value());
+    return MakeAutoDiffScalar(
+        m_value / other.value(),
+        has_both_der ?
+            VectorXd(this_der * other.value() - other_der * m_value) * scale :
+        has_this_der ?
+            VectorXd(this_der * other.value()) * scale :
+        // has_other_der || has_neither
+            VectorXd(other_der * -m_value) * scale);
+  }
+
+  template <typename OtherDerType>
+  inline const AutoDiffScalar<DerType> operator*(
+      const AutoDiffScalar<OtherDerType>& other) const {
+    const bool has_this_der = m_derivatives.size() > 0;
+    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
+    return MakeAutoDiffScalar(
+        m_value * other.value(),
+        has_both_der ? VectorXd(m_derivatives * other.value() +
+                                other.derivatives() * m_value)
+                     : has_this_der ? VectorXd(m_derivatives * other.value())
+                                    : VectorXd(other.derivatives() * m_value));
+  }
+
+  inline AutoDiffScalar& operator*=(const Scalar& other) {
+    *this = *this * other;
+    return *this;
+  }
+
+  template <typename OtherDerType>
+  inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other) {
+    *this = *this * other;
+    return *this;
+  }
+
+  inline AutoDiffScalar& operator/=(const Scalar& other) {
+    *this = *this / other;
+    return *this;
+  }
+
+  template <typename OtherDerType>
+  inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other) {
+    *this = *this / other;
+    return *this;
+  }
+
+ protected:
+  Scalar m_value;
+  DerType m_derivatives;
+};
+
+#define DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(FUNC, CODE) \
+  inline const AutoDiffScalar<VectorXd> FUNC(                   \
+      const AutoDiffScalar<VectorXd>& x) {                      \
+    EIGEN_UNUSED typedef double Scalar;                         \
+    CODE;                                                       \
+  }
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    abs, using std::abs; return Eigen::MakeAutoDiffScalar(
+        abs(x.value()), x.derivatives() * (x.value() < 0 ? -1 : 1));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    abs2, using numext::abs2; return Eigen::MakeAutoDiffScalar(
+        abs2(x.value()), x.derivatives() * (Scalar(2) * x.value()));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    sqrt, using std::sqrt; Scalar sqrtx = sqrt(x.value());
+    return Eigen::MakeAutoDiffScalar(sqrtx,
+                                     x.derivatives() * (Scalar(0.5) / sqrtx));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    cos, using std::cos; using std::sin;
+    return Eigen::MakeAutoDiffScalar(cos(x.value()),
+                                     x.derivatives() * (-sin(x.value())));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    sin, using std::sin; using std::cos;
+    return Eigen::MakeAutoDiffScalar(sin(x.value()),
+                                     x.derivatives() * cos(x.value()));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    exp, using std::exp; Scalar expx = exp(x.value());
+    return Eigen::MakeAutoDiffScalar(expx, x.derivatives() * expx);)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    log, using std::log; return Eigen::MakeAutoDiffScalar(
+        log(x.value()), x.derivatives() * (Scalar(1) / x.value()));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    tan, using std::tan; using std::cos; return Eigen::MakeAutoDiffScalar(
+        tan(x.value()),
+        x.derivatives() * (Scalar(1) / numext::abs2(cos(x.value()))));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    asin, using std::sqrt; using std::asin; return Eigen::MakeAutoDiffScalar(
+        asin(x.value()),
+        x.derivatives() * (Scalar(1) / sqrt(1 - numext::abs2(x.value()))));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    acos, using std::sqrt; using std::acos; return Eigen::MakeAutoDiffScalar(
+        acos(x.value()),
+        x.derivatives() * (Scalar(-1) / sqrt(1 - numext::abs2(x.value()))));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    atan, using std::atan; return Eigen::MakeAutoDiffScalar(
+        atan(x.value()),
+        x.derivatives() * (Scalar(1) / (1 + x.value() * x.value())));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    tanh, using std::cosh; using std::tanh; return Eigen::MakeAutoDiffScalar(
+        tanh(x.value()),
+        x.derivatives() * (Scalar(1) / numext::abs2(cosh(x.value()))));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    sinh, using std::sinh; using std::cosh;
+    return Eigen::MakeAutoDiffScalar(sinh(x.value()),
+                                     x.derivatives() * cosh(x.value()));)
+
+DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
+    cosh, using std::sinh; using std::cosh;
+    return Eigen::MakeAutoDiffScalar(cosh(x.value()),
+                                     x.derivatives() * sinh(x.value()));)
+
+#undef DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY
+
+// We have this specialization here because the Eigen-3.3.3's atan2
+// implementation for AutoDiffScalar does not make a return with properly sized
+// derivatives.
+inline const AutoDiffScalar<VectorXd> atan2(const AutoDiffScalar<VectorXd>& a,
+                                            const AutoDiffScalar<VectorXd>& b) {
+  const bool has_a_der = a.derivatives().size() > 0;
+  const bool has_both_der = has_a_der && (b.derivatives().size() > 0);
+  const double squared_hypot = a.value() * a.value() + b.value() * b.value();
+  return MakeAutoDiffScalar(
+      std::atan2(a.value(), b.value()),
+      VectorXd((has_both_der
+                    ? VectorXd(a.derivatives() * b.value() -
+                               a.value() * b.derivatives())
+                    : has_a_der ? VectorXd(a.derivatives() * b.value())
+                                : VectorXd(-a.value() * b.derivatives())) /
+               squared_hypot));
+}
+
+inline const AutoDiffScalar<VectorXd> pow(const AutoDiffScalar<VectorXd>& a,
+                                          double b) {
+  using std::pow;
+  return MakeAutoDiffScalar(pow(a.value(), b),
+                            a.derivatives() * (b * pow(a.value(), b - 1)));
+}
+
+#endif
+
+}  // namespace Eigen

wpimath/src/test/native/include/drake/common/cond.h

diff --git a/wpimath/src/test/native/include/drake/common/cond.h b/wpimath/src/test/native/include/drake/common/cond.h
new file mode 100644
index 0000000..16dd21e
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/cond.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <functional>
+#include <type_traits>
+
+#include "drake/common/double_overloads.h"
+
+namespace drake {
+/** @name cond
+  Constructs conditional expression (similar to Lisp's cond).
+
+  @verbatim
+    cond(cond_1, expr_1,
+         cond_2, expr_2,
+            ...,   ...,
+         cond_n, expr_n,
+         expr_{n+1})
+  @endverbatim
+
+  The value returned by the above cond expression is @c expr_1 if @c cond_1 is
+  true; else if @c cond_2 is true then @c expr_2; ... ; else if @c cond_n is
+  true then @c expr_n. If none of the conditions are true, it returns @c
+  expr_{n+1}.
+
+  @note This functions assumes that @p ScalarType provides @c operator< and the
+  type of @c f_cond is the type of the return type of <tt>operator<(ScalarType,
+  ScalarType)</tt>. For example, @c symbolic::Expression can be used as a @p
+  ScalarType because it provides <tt>symbolic::Formula
+  operator<(symbolic::Expression, symbolic::Expression)</tt>.
+
+
+  @{
+ */
+template <typename ScalarType>
+ScalarType cond(const ScalarType& e) {
+  return e;
+}
+template <typename ScalarType, typename... Rest>
+ScalarType cond(const decltype(ScalarType() < ScalarType()) & f_cond,
+                const ScalarType& e_then, Rest... rest) {
+  return if_then_else(f_cond, e_then, cond(rest...));
+}
+///@}
+}  // namespace drake

wpimath/src/test/native/include/drake/common/constants.h

diff --git a/wpimath/src/test/native/include/drake/common/constants.h b/wpimath/src/test/native/include/drake/common/constants.h
new file mode 100644
index 0000000..0ccddca
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/constants.h
@@ -0,0 +1,19 @@
+#pragma once
+
+namespace drake {
+
+constexpr int kQuaternionSize = 4;
+
+constexpr int kSpaceDimension = 3;
+
+constexpr int kRpySize = 3;
+
+/// https://en.wikipedia.org/wiki/Screw_theory#Twist
+constexpr int kTwistSize = 6;
+
+/// http://www.euclideanspace.com/maths/geometry/affine/matrix4x4/
+constexpr int kHomogeneousTransformSize = 16;
+
+const int kRotmatSize = kSpaceDimension * kSpaceDimension;
+
+}  // namespace drake

wpimath/src/test/native/include/drake/common/double_overloads.h

diff --git a/wpimath/src/test/native/include/drake/common/double_overloads.h b/wpimath/src/test/native/include/drake/common/double_overloads.h
new file mode 100644
index 0000000..125f113
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/double_overloads.h
@@ -0,0 +1,21 @@
+/// @file
+/// Provides necessary operations on double to have it as a ScalarType in drake.
+
+#pragma once
+
+namespace drake {
+/// Provides if-then-else expression for double. The value returned by the
+/// if-then-else expression is @p v_then if @p f_cond is @c true. Otherwise, it
+/// returns @p v_else.
+
+/// The semantics is similar but not exactly the same as C++'s conditional
+/// expression constructed by its ternary operator, @c ?:. In
+/// <tt>if_then_else(f_cond, v_then, v_else)</tt>, both of @p v_then and @p
+/// v_else are evaluated regardless of the evaluation of @p f_cond. In contrast,
+/// only one of @p v_then or @p v_else is evaluated in C++'s conditional
+/// expression <tt>f_cond ? v_then : v_else</tt>.
+inline double if_then_else(bool f_cond, double v_then, double v_else) {
+  return f_cond ? v_then : v_else;
+}
+
+}  // namespace drake

wpimath/src/test/native/include/drake/common/drake_deprecated.h

diff --git a/wpimath/src/test/native/include/drake/common/drake_deprecated.h b/wpimath/src/test/native/include/drake/common/drake_deprecated.h
new file mode 100644
index 0000000..5ce6328
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/drake_deprecated.h
@@ -0,0 +1,65 @@
+#pragma once
+
+/** @file
+Provides a portable macro for use in generating compile-time warnings for
+use of code that is permitted but discouraged. */
+
+#ifdef DRAKE_DOXYGEN_CXX
+/** Use `DRAKE_DEPRECATED("removal_date", "message")` to discourage use of
+certain APIs.  It can be used on classes, typedefs, variables, non-static data
+members, functions, arguments, enumerations, and template specializations. When
+code refers to the deprecated item, a compile time warning will be issued
+displaying the given message, preceded by "DRAKE DEPRECATED: ". The Doxygen API
+reference will show that the API is deprecated, along with the message.
+
+This is typically used for constructs that have been replaced by something
+better and it is good practice to suggest the appropriate replacement in the
+deprecation message. Deprecation warnings are conventionally used to convey to
+users that a feature they are depending on will be removed in a future release.
+
+Every deprecation notice must include a date (as YYYY-MM-DD string) where the
+deprecated item is planned for removal.  Future commits may change the date
+(e.g., delaying the removal) but should generally always reflect the best
+current expectation for removal.
+
+Absent any other particular need, we prefer to use a deprecation period of
+three months by default, often rounded up to the next first of the month.  So
+for code announced as deprecated on 2018-01-22 the removal_date would nominally
+be set to 2018-05-01.
+
+Try to keep the date string immediately after the DRAKE_DEPRECATED macro name,
+even if the message itself must be wrapped to a new line:
+@code
+  DRAKE_DEPRECATED("2038-01-19",
+      "foo is being replaced with a safer, const-method named bar().")
+  int foo();
+@endcode
+
+Sample uses: @code
+  // Attribute comes *before* declaration of a deprecated function or variable;
+  // no semicolon is allowed.
+  DRAKE_DEPRECATED("2038-01-19", "f() is slow; use g() instead.")
+  int f(int arg);
+
+  // Attribute comes *after* struct, class, enum keyword.
+  class DRAKE_DEPRECATED("2038-01-19", "Use MyNewClass instead.")
+  MyClass {
+  };
+
+  // Type alias goes before the '='.
+  using NewType
+      DRAKE_DEPRECATED("2038-01-19", "Use NewType instead.")
+      = OldType;
+@endcode
+*/
+#define DRAKE_DEPRECATED(removal_date, message)
+
+#else  // DRAKE_DOXYGEN_CXX
+
+#define DRAKE_DEPRECATED(removal_date, message)         \
+  [[deprecated(                                         \
+  "\nDRAKE DEPRECATED: " message                        \
+  "\nThe deprecated code will be removed from Drake"    \
+  " on or after " removal_date ".")]]
+
+#endif  // DRAKE_DOXYGEN_CXX

wpimath/src/test/native/include/drake/common/drake_nodiscard.h

diff --git a/wpimath/src/test/native/include/drake/common/drake_nodiscard.h b/wpimath/src/test/native/include/drake/common/drake_nodiscard.h
new file mode 100644
index 0000000..29f078d
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/drake_nodiscard.h
@@ -0,0 +1,13 @@
+#pragma once
+
+// TODO(jwnimmer-tri) Once we are in --std=c++17 mode as our minimum version,
+// we can remove this file and just say [[nodiscard]] directly everywhere.
+
+#if defined(DRAKE_DOXYGEN_CXX) || __has_cpp_attribute(nodiscard)
+/** Synonym for [[nodiscard]], iff the current compiler supports it;
+see https://en.cppreference.com/w/cpp/language/attributes/nodiscard. */
+// NOLINTNEXTLINE(whitespace/braces)
+#define DRAKE_NODISCARD [[nodiscard]]
+#else
+#define DRAKE_NODISCARD
+#endif

wpimath/src/test/native/include/drake/common/dummy_value.h

diff --git a/wpimath/src/test/native/include/drake/common/dummy_value.h b/wpimath/src/test/native/include/drake/common/dummy_value.h
new file mode 100644
index 0000000..b9c616a
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/dummy_value.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <limits>
+
+namespace drake {
+
+/// Provides a "dummy" value for a ScalarType -- a value that is unlikely to be
+/// mistaken for a purposefully-computed value, useful for initializing a value
+/// before the true result is available.
+///
+/// Defaults to using std::numeric_limits::quiet_NaN when available; it is a
+/// compile-time error to call the unspecialized dummy_value::get() when
+/// quiet_NaN is unavailable.
+///
+/// See autodiff_overloads.h to use this with Eigen's AutoDiffScalar.
+template <typename T>
+struct dummy_value {
+  static constexpr T get() {
+    static_assert(std::numeric_limits<T>::has_quiet_NaN,
+                  "Custom scalar types should specialize this struct");
+    return std::numeric_limits<T>::quiet_NaN();
+  }
+};
+
+template <>
+struct dummy_value<int> {
+  static constexpr int get() {
+    // D is for "Dummy".  We assume as least 32 bits (per cppguide) -- if `int`
+    // is larger than 32 bits, this will leave some fraction of the bytes zero
+    // instead of 0xDD, but that's okay.
+    return 0xDDDDDDDD;
+  }
+};
+
+}  // namespace drake

wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h

diff --git a/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h
new file mode 100644
index 0000000..49175ce
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#ifndef DRAKE_COMMON_AUTODIFF_HEADER
+// TODO(soonho-tri): Change to #error.
+#warning Do not directly include this file. Include "drake/common/autodiff.h".
+#endif
+
+#include <limits>
+
+// Eigen provides `numeric_limits<AutoDiffScalar<T>>` starting with v3.3.4.
+#if !EIGEN_VERSION_AT_LEAST(3, 3, 4)  // Eigen Version < v3.3.4
+
+namespace std {
+template <typename T>
+class numeric_limits<Eigen::AutoDiffScalar<T>>
+    : public numeric_limits<typename T::Scalar> {};
+
+}  // namespace std
+
+#endif  // Eigen Version < v3.3.4

wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h

diff --git a/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h b/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h
new file mode 100644
index 0000000..10ffec6
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h
@@ -0,0 +1,38 @@
+#pragma once
+
+/// @file
+/// This file contains abbreviated definitions for certain uses of
+/// AutoDiffScalar that are commonly used in Drake.
+/// @see also eigen_types.h
+
+#ifndef DRAKE_COMMON_AUTODIFF_HEADER
+// TODO(soonho-tri): Change to #error.
+#warning Do not directly include this file. Include "drake/common/autodiff.h".
+#endif
+
+#include <type_traits>
+
+#include <Eigen/Core>
+
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+
+/// An autodiff variable with a dynamic number of partials.
+using AutoDiffXd = Eigen::AutoDiffScalar<Eigen::VectorXd>;
+
+// TODO(hongkai-dai): Recursive template to get arbitrary gradient order.
+
+/// An autodiff variable with `num_vars` partials.
+template <int num_vars>
+using AutoDiffd = Eigen::AutoDiffScalar<Eigen::Matrix<double, num_vars, 1> >;
+
+/// A vector of `rows` autodiff variables, each with `num_vars` partials.
+template <int num_vars, int rows>
+using AutoDiffVecd = Eigen::Matrix<AutoDiffd<num_vars>, rows, 1>;
+
+/// A dynamic-sized vector of autodiff variables, each with a dynamic-sized
+/// vector of partials.
+typedef AutoDiffVecd<Eigen::Dynamic, Eigen::Dynamic> AutoDiffVecXd;
+
+}  // namespace drake

wpimath/src/test/native/include/drake/common/eigen_types.h

diff --git a/wpimath/src/test/native/include/drake/common/eigen_types.h b/wpimath/src/test/native/include/drake/common/eigen_types.h
new file mode 100644
index 0000000..abe3e0b
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/eigen_types.h
@@ -0,0 +1,461 @@
+#pragma once
+
+/// @file
+/// This file contains abbreviated definitions for certain specializations of
+/// Eigen::Matrix that are commonly used in Drake.
+/// These convenient definitions are templated on the scalar type of the Eigen
+/// object. While Drake uses `<T>` for scalar types across the entire code base
+/// we decided in this file to use `<Scalar>` to be more consistent with the
+/// usage of `<Scalar>` in Eigen's code base.
+/// @see also eigen_autodiff_types.h
+
+#include <utility>
+
+#include <Eigen/Core>
+
+#include "drake/common/constants.h"
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_copyable.h"
+#include "drake/common/drake_deprecated.h"
+
+namespace drake {
+
+/// The empty column vector (zero rows, one column), templated on scalar type.
+template <typename Scalar>
+using Vector0 = Eigen::Matrix<Scalar, 0, 1>;
+
+/// A column vector of size 1 (that is, a scalar), templated on scalar type.
+template <typename Scalar>
+using Vector1 = Eigen::Matrix<Scalar, 1, 1>;
+
+/// A column vector of size 1 of doubles.
+using Vector1d = Eigen::Matrix<double, 1, 1>;
+
+/// A column vector of size 2, templated on scalar type.
+template <typename Scalar>
+using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
+
+/// A column vector of size 3, templated on scalar type.
+template <typename Scalar>
+using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
+
+/// A column vector of size 4, templated on scalar type.
+template <typename Scalar>
+using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
+
+/// A column vector of size 6.
+template <typename Scalar>
+using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
+
+/// A column vector templated on the number of rows.
+template <typename Scalar, int Rows>
+using Vector = Eigen::Matrix<Scalar, Rows, 1>;
+
+/// A column vector of any size, templated on scalar type.
+template <typename Scalar>
+using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+/// A vector of dynamic size templated on scalar type, up to a maximum of 6
+/// elements.
+template <typename Scalar>
+using VectorUpTo6 = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, 6, 1>;
+
+/// A row vector of size 2, templated on scalar type.
+template <typename Scalar>
+using RowVector2 = Eigen::Matrix<Scalar, 1, 2>;
+
+/// A row vector of size 3, templated on scalar type.
+template <typename Scalar>
+using RowVector3 = Eigen::Matrix<Scalar, 1, 3>;
+
+/// A row vector of size 4, templated on scalar type.
+template <typename Scalar>
+using RowVector4 = Eigen::Matrix<Scalar, 1, 4>;
+
+/// A row vector of size 6.
+template <typename Scalar>
+using RowVector6 = Eigen::Matrix<Scalar, 1, 6>;
+
+/// A row vector templated on the number of columns.
+template <typename Scalar, int Cols>
+using RowVector = Eigen::Matrix<Scalar, 1, Cols>;
+
+/// A row vector of any size, templated on scalar type.
+template <typename Scalar>
+using RowVectorX = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;
+
+
+/// A matrix of 2 rows and 2 columns, templated on scalar type.
+template <typename Scalar>
+using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;
+
+/// A matrix of 3 rows and 3 columns, templated on scalar type.
+template <typename Scalar>
+using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
+
+/// A matrix of 4 rows and 4 columns, templated on scalar type.
+template <typename Scalar>
+using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
+
+/// A matrix of 6 rows and 6 columns, templated on scalar type.
+template <typename Scalar>
+using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
+
+/// A matrix of 2 rows, dynamic columns, templated on scalar type.
+template <typename Scalar>
+using Matrix2X = Eigen::Matrix<Scalar, 2, Eigen::Dynamic>;
+
+/// A matrix of 3 rows, dynamic columns, templated on scalar type.
+template <typename Scalar>
+using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
+
+/// A matrix of 4 rows, dynamic columns, templated on scalar type.
+template <typename Scalar>
+using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
+
+/// A matrix of 6 rows, dynamic columns, templated on scalar type.
+template <typename Scalar>
+using Matrix6X = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>;
+
+/// A matrix of dynamic size, templated on scalar type.
+template <typename Scalar>
+using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+
+/// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows
+/// and 6 columns. Rectangular matrices, with different number of rows and
+/// columns, are allowed.
+template <typename Scalar>
+using MatrixUpTo6 =
+Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6>;
+
+/// A quaternion templated on scalar type.
+template <typename Scalar>
+using Quaternion = Eigen::Quaternion<Scalar>;
+
+/// An AngleAxis templated on scalar type.
+template <typename Scalar>
+using AngleAxis = Eigen::AngleAxis<Scalar>;
+
+/// An Isometry templated on scalar type.
+template <typename Scalar>
+using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
+
+/// A translation in 3D templated on scalar type.
+template <typename Scalar>
+using Translation3 = Eigen::Translation<Scalar, 3>;
+
+/// A column vector consisting of one twist.
+template <typename Scalar>
+using TwistVector = Eigen::Matrix<Scalar, kTwistSize, 1>;
+
+/// A matrix with one twist per column, and dynamically many columns.
+template <typename Scalar>
+using TwistMatrix = Eigen::Matrix<Scalar, kTwistSize, Eigen::Dynamic>;
+
+/// A six-by-six matrix.
+template <typename Scalar>
+using SquareTwistMatrix = Eigen::Matrix<Scalar, kTwistSize, kTwistSize>;
+
+/// A column vector consisting of one wrench (spatial force) = `[r X f; f]`,
+/// where f is a force (translational force) applied at a point `P` and `r` is
+/// the position vector from a point `O` (called the "moment center") to point
+/// `P`.
+template <typename Scalar>
+using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
+
+/// A column vector consisting of a concatenated rotational and translational
+/// force.  The wrench is a special case of a SpatialForce.  For a general
+/// SpatialForce the rotational force can be a pure torque or the accumulation
+/// of moments and need not necessarily be a function of the force term.
+template <typename Scalar>
+using SpatialForce
+DRAKE_DEPRECATED("2019-10-15", "Please use Vector6<> instead.")
+    = Eigen::Matrix<Scalar, 6, 1>;
+
+/// EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time
+/// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by
+/// Dynamic, followed by other finite values.
+///
+/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_DYNAMIC
+/// macro in "Eigen/Core/util/Macros.h".
+template <int a, int b>
+struct EigenSizeMinPreferDynamic {
+  // clang-format off
+  static constexpr int value = (a == 0 || b == 0) ? 0 :
+                               (a == 1 || b == 1) ? 1 :
+     (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic :
+                                           a <= b ? a : b;
+  // clang-format on
+};
+
+/// EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic. The
+/// difference is that finite values now have priority over Dynamic, so that
+/// EigenSizeMinPreferFixed<3, Dynamic>::value gives 3.
+///
+/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_FIXED macro
+/// in "Eigen/Core/util/Macros.h".
+template <int a, int b>
+struct EigenSizeMinPreferFixed {
+  // clang-format off
+  static constexpr int value = (a == 0 || b == 0) ? 0 :
+                               (a == 1 || b == 1) ? 1 :
+     (a == Eigen::Dynamic && b == Eigen::Dynamic) ? Eigen::Dynamic :
+                            (a == Eigen::Dynamic) ? b :
+                            (b == Eigen::Dynamic) ? a :
+                                           a <= b ? a : b;
+  // clang-format on
+};
+
+/// MultiplyEigenSizes<a, b> gives a * b if both of a and b are fixed
+/// sizes. Otherwise it gives Eigen::Dynamic.
+template <int a, int b>
+struct MultiplyEigenSizes {
+  static constexpr int value =
+      (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : a * b;
+};
+
+/*
+ * Determines if a type is derived from EigenBase<> (e.g. ArrayBase<>,
+ * MatrixBase<>).
+ */
+template <typename Derived>
+struct is_eigen_type : std::is_base_of<Eigen::EigenBase<Derived>, Derived> {};
+
+/*
+ * Determines if an EigenBase<> has a specific scalar type.
+ */
+template <typename Derived, typename Scalar>
+struct is_eigen_scalar_same
+    : std::integral_constant<
+          bool, is_eigen_type<Derived>::value &&
+                    std::is_same<typename Derived::Scalar, Scalar>::value> {};
+
+/*
+ * Determines if an EigenBase<> type is a compile-time (column) vector.
+ * This will not check for run-time size.
+ */
+template <typename Derived>
+struct is_eigen_vector
+    : std::integral_constant<bool, is_eigen_type<Derived>::value &&
+                                       Derived::ColsAtCompileTime == 1> {};
+
+/*
+ * Determines if an EigenBase<> type is a compile-time (column) vector of a
+ * scalar type. This will not check for run-time size.
+ */
+template <typename Derived, typename Scalar>
+struct is_eigen_vector_of
+    : std::integral_constant<
+          bool, is_eigen_scalar_same<Derived, Scalar>::value &&
+                    is_eigen_vector<Derived>::value> {};
+
+// TODO(eric.cousineau): A 1x1 matrix will be disqualified in this case, and
+// this logic will qualify it as a vector. Address the downstream logic if this
+// becomes an issue.
+/*
+ * Determines if a EigenBase<> type is a compile-time non-column-vector matrix
+ * of a scalar type. This will not check for run-time size.
+ * @note For an EigenBase<> of the correct Scalar type, this logic is
+ * exclusive to is_eigen_vector_of<> such that distinct specializations are not
+ * ambiguous.
+ */
+template <typename Derived, typename Scalar>
+struct is_eigen_nonvector_of
+    : std::integral_constant<
+          bool, is_eigen_scalar_same<Derived, Scalar>::value &&
+                    !is_eigen_vector<Derived>::value> {};
+
+// TODO(eric.cousineau): Add alias is_eigen_matrix_of = is_eigen_scalar_same if
+// appropriate.
+
+/// This wrapper class provides a way to write non-template functions taking raw
+/// pointers to Eigen objects as parameters while limiting the number of copies,
+/// similar to `Eigen::Ref`. Internally, it keeps an instance of `Eigen::Ref<T>`
+/// and provides access to it via `operator*` and `operator->`.
+///
+/// The motivation of this class is to follow <a
+/// href="https://google.github.io/styleguide/cppguide.html#Reference_Arguments">GSG's
+/// "output arguments should be pointers" rule</a> while taking advantage of
+/// using `Eigen::Ref`. Here is an example.
+///
+/// @code
+/// // This function is taking an Eigen::Ref of a matrix and modifies it in
+/// // the body. This violates GSG's rule on output parameters.
+/// void foo(Eigen::Ref<Eigen::MatrixXd> M) {
+///    M(0, 0) = 0;
+/// }
+/// // At Call-site, we have:
+/// foo(M);
+/// foo(M.block(0, 0, 2, 2));
+///
+/// // We can rewrite the above function into the following using EigenPtr.
+/// void foo(EigenPtr<Eigen::MatrixXd> M) {
+///    (*M)(0, 0) = 0;
+/// }
+/// // Note that, call sites should be changed to:
+/// foo(&M);
+///
+/// // We need tmp to avoid taking the address of a temporary object such as the
+/// // return value of .block().
+/// auto tmp = M.block(0, 0, 2, 2);
+/// foo(&tmp);
+/// @endcode
+///
+/// Notice that methods taking an EigenPtr can mutate the entries of a matrix as
+/// in method `foo()` in the example code above, but cannot change its size.
+/// This is because `operator*` and `operator->` return an `Eigen::Ref<T>`
+/// object and only plain matrices/arrays can be resized and not expressions.
+/// This **is** the desired behavior, since resizing the block of a matrix or
+/// even a more general expression should not be allowed. If you do want to be
+/// able to resize a mutable matrix argument, then you must pass it as a
+/// `Matrix<T>*`, like so:
+/// @code
+/// void bar(Eigen::MatrixXd* M) {
+///   DRAKE_THROW_UNLESS(M != nullptr);
+///   // In this case this method only works with 4x3 matrices.
+///   if (M->rows() != 4 && M->cols() != 3) {
+///     M->resize(4, 3);
+///   }
+///   (*M)(0, 0) = 0;
+/// }
+/// @endcode
+///
+/// @note This class provides a way to avoid the `const_cast` hack introduced in
+/// <a
+/// href="https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#TopicPlainFunctionsFailing">Eigen's
+/// documentation</a>.
+template <typename PlainObjectType>
+class EigenPtr {
+ public:
+  typedef Eigen::Ref<PlainObjectType> RefType;
+
+  EigenPtr() : EigenPtr(nullptr) {}
+
+  /// Overload for `nullptr`.
+  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
+  EigenPtr(std::nullptr_t) {}
+
+  /// Constructs with a reference to the given matrix type.
+  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
+  EigenPtr(const EigenPtr& other) { assign(other); }
+
+  /// Constructs with a reference to another matrix type.
+  /// May be `nullptr`.
+  template <typename PlainObjectTypeIn>
+  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
+  EigenPtr(PlainObjectTypeIn* m) {
+    if (m) {
+      m_.set_value(m);
+    }
+  }
+
+  /// Constructs from another EigenPtr.
+  template <typename PlainObjectTypeIn>
+  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
+  EigenPtr(const EigenPtr<PlainObjectTypeIn>& other) {
+    // Cannot directly construct `m_` from `other.m_`.
+    assign(other);
+  }
+
+  EigenPtr& operator=(const EigenPtr& other) {
+    // We must explicitly override this version of operator=.
+    // The template below will not take precedence over this one.
+    return assign(other);
+  }
+
+  template <typename PlainObjectTypeIn>
+  EigenPtr& operator=(const EigenPtr<PlainObjectTypeIn>& other) {
+    return assign(other);
+  }
+
+  /// @throws std::runtime_error if this is a null dereference.
+  RefType& operator*() const { return get_reference(); }
+
+  /// @throws std::runtime_error if this is a null dereference.
+  RefType* operator->() const { return &get_reference(); }
+
+  /// Returns whether or not this contains a valid reference.
+  operator bool() const { return is_valid(); }
+
+  bool operator==(std::nullptr_t) const { return !is_valid(); }
+
+  bool operator!=(std::nullptr_t) const { return is_valid(); }
+
+ private:
+  // Simple reassignable container without requirement of heap allocation.
+  // This is used because `drake::optional<>` does not work with `Eigen::Ref<>`
+  // because `Ref` deletes the necessary `operator=` overload for
+  // `std::is_copy_assignable`.
+  class ReassignableRef {
+   public:
+    DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ReassignableRef)
+    ReassignableRef() {}
+    ~ReassignableRef() {
+      reset();
+    }
+
+    // Reset value to null.
+    void reset() {
+      if (has_value_) {
+        raw_value().~RefType();
+        has_value_ = false;
+      }
+    }
+
+    // Set value.
+    template <typename PlainObjectTypeIn>
+    void set_value(PlainObjectTypeIn* value_in) {
+      if (has_value_) {
+        raw_value().~RefType();
+      }
+      new (&raw_value()) RefType(*value_in);
+      has_value_ = true;
+    }
+
+    // Access to value.
+    RefType& value() {
+      DRAKE_ASSERT(has_value());
+      return raw_value();
+    }
+
+    // Indicates if it has a value.
+    bool has_value() const { return has_value_; }
+
+   private:
+    // Unsafe access to value.
+    RefType& raw_value() { return reinterpret_cast<RefType&>(storage_); }
+
+    bool has_value_{};
+    typename std::aligned_storage<sizeof(RefType), alignof(RefType)>::type
+        storage_;
+  };
+
+  // Use mutable, reassignable ref to permit pointer-like semantics (with
+  // ownership) on the stack.
+  mutable ReassignableRef m_;
+
+  // Consolidate assignment here, so that both the copy constructor and the
+  // construction from another type may be used.
+  template <typename PlainObjectTypeIn>
+  EigenPtr& assign(const EigenPtr<PlainObjectTypeIn>& other) {
+    if (other) {
+      m_.set_value(&(*other));
+    } else {
+      m_.reset();
+    }
+    return *this;
+  }
+
+  // Consolidate getting a reference here.
+  RefType& get_reference() const {
+    if (!m_.has_value())
+      throw std::runtime_error("EigenPtr: nullptr dereference");
+    return m_.value();
+  }
+
+  bool is_valid() const {
+    return m_.has_value();
+  }
+};
+
+}  // namespace drake

wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h

diff --git a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
new file mode 100644
index 0000000..8847d87
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
@@ -0,0 +1,116 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+#include "drake/common/drake_nodiscard.h"
+
+namespace drake {
+
+enum class MatrixCompareType { absolute, relative };
+
+/**
+ * Compares two matrices to determine whether they are equal to within a certain
+ * threshold.
+ *
+ * @param m1 The first matrix to compare.
+ * @param m2 The second matrix to compare.
+ * @param tolerance The tolerance for determining equivalence.
+ * @param compare_type Whether the tolereance is absolute or relative.
+ * @param explanation A pointer to a string variable for saving an explanation
+ * of why @p m1 and @p m2 are unequal. This parameter is optional and defaults
+ * to `nullptr`. If this is `nullptr` and @p m1 and @p m2 are not equal, an
+ * explanation is logged as an error message.
+ * @return true if the two matrices are equal based on the specified tolerance.
+ */
+template <typename DerivedA, typename DerivedB>
+DRAKE_NODISCARD ::testing::AssertionResult CompareMatrices(
+    const Eigen::MatrixBase<DerivedA>& m1,
+    const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
+    MatrixCompareType compare_type = MatrixCompareType::absolute) {
+  if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
+    return ::testing::AssertionFailure()
+           << "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
+           << " vs. " << m2.rows() << " x " << m2.cols() << ")";
+  }
+
+  for (int ii = 0; ii < m1.rows(); ii++) {
+    for (int jj = 0; jj < m1.cols(); jj++) {
+      // First handle the corner cases of positive infinity, negative infinity,
+      // and NaN
+      const auto both_positive_infinity =
+          m1(ii, jj) == std::numeric_limits<double>::infinity() &&
+          m2(ii, jj) == std::numeric_limits<double>::infinity();
+
+      const auto both_negative_infinity =
+          m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
+          m2(ii, jj) == -std::numeric_limits<double>::infinity();
+
+      using std::isnan;
+      const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj));
+
+      if (both_positive_infinity || both_negative_infinity || both_nan)
+        continue;
+
+      // Check for case where one value is NaN and the other is not
+      if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
+          (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
+        return ::testing::AssertionFailure() << "NaN mismatch at (" << ii
+                                             << ", " << jj << "):\nm1 =\n"
+                                             << m1 << "\nm2 =\n"
+                                             << m2;
+      }
+
+      // Determine whether the difference between the two matrices is less than
+      // the tolerance.
+      using std::abs;
+      const auto delta = abs(m1(ii, jj) - m2(ii, jj));
+
+      if (compare_type == MatrixCompareType::absolute) {
+        // Perform comparison using absolute tolerance.
+
+        if (delta > tolerance) {
+          return ::testing::AssertionFailure()
+                 << "Values at (" << ii << ", " << jj
+                 << ") exceed tolerance: " << m1(ii, jj) << " vs. "
+                 << m2(ii, jj) << ", diff = " << delta
+                 << ", tolerance = " << tolerance << "\nm1 =\n"
+                 << m1 << "\nm2 =\n"
+                 << m2 << "\ndelta=\n"
+                 << (m1 - m2);
+        }
+      } else {
+        // Perform comparison using relative tolerance, see:
+        // http://realtimecollisiondetection.net/blog/?p=89
+        using std::max;
+        const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
+        const auto relative_tolerance =
+            tolerance * max(decltype(max_value){1}, max_value);
+
+        if (delta > relative_tolerance) {
+          return ::testing::AssertionFailure()
+                 << "Values at (" << ii << ", " << jj
+                 << ") exceed tolerance: " << m1(ii, jj) << " vs. "
+                 << m2(ii, jj) << ", diff = " << delta
+                 << ", tolerance = " << tolerance
+                 << ", relative tolerance = " << relative_tolerance
+                 << "\nm1 =\n"
+                 << m1 << "\nm2 =\n"
+                 << m2 << "\ndelta=\n"
+                 << (m1 - m2);
+        }
+      }
+    }
+  }
+
+  return ::testing::AssertionSuccess() << "m1 =\n"
+                                       << m1
+                                       << "\nis approximately equal to m2 =\n"
+                                       << m2;
+}
+
+}  // namespace drake

wpimath/src/test/native/include/drake/common/unused.h

diff --git a/wpimath/src/test/native/include/drake/common/unused.h b/wpimath/src/test/native/include/drake/common/unused.h
new file mode 100644
index 0000000..5a28b01
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/common/unused.h
@@ -0,0 +1,53 @@
+#pragma once
+
+namespace drake {
+
+/// Documents the argument(s) as unused, placating GCC's -Wunused-parameter
+/// warning.  This can be called within function bodies to mark that certain
+/// parameters are unused.
+///
+/// When possible, removing the unused parameter is better than placating the
+/// warning.  However, in some cases the parameter is part of a virtual API or
+/// template concept that is used elsewhere, so we can't remove it.  In those
+/// cases, this function might be an appropriate work-around.
+///
+/// Here's rough advice on how to fix Wunused-parameter warnings:
+///
+/// (1) If the parameter can be removed entirely, prefer that as the first
+///     choice.  (This may not be possible if, e.g., a method must match some
+///     virtual API or template concept.)
+///
+/// (2) Unless the parameter name has acute value, prefer to omit the name of
+///     the parameter, leaving only the type, e.g.
+/// @code
+/// void Print(const State& state) override { /* No state to print. */ }
+/// @endcode
+///     changes to
+/// @code
+/// void Print(const State&) override { /* No state to print. */}
+/// @endcode
+///     This no longer triggers the warning and further makes it clear that a
+///     parameter required by the API is definitively unused in the function.
+///
+///     This is an especially good solution in the context of method
+///     definitions (vs declarations); the parameter name used in a definition
+///     is entirely irrelevant to Doxygen and most readers.
+///
+/// (3) When leaving the parameter name intact has acute value, it is
+///     acceptable to keep the name and mark it `unused`.  For example, when
+///     the name appears as part of a virtual method's base class declaration,
+///     the name is used by Doxygen to document the method, e.g.,
+/// @code
+/// /** Sets the default State of a System.  This default implementation is to
+///     set all zeros.  Subclasses may override to use non-zero defaults.  The
+///     custom defaults may be based on the given @p context, when relevant.  */
+/// virtual void SetDefault(const Context<T>& context, State<T>* state) const {
+///   unused(context);
+///   state->SetZero();
+/// }
+/// @endcode
+///
+template <typename ... Args>
+void unused(const Args& ...) {}
+
+}  // namespace drake

wpimath/src/test/native/include/drake/math/autodiff.h

diff --git a/wpimath/src/test/native/include/drake/math/autodiff.h b/wpimath/src/test/native/include/drake/math/autodiff.h
new file mode 100644
index 0000000..52edb11
--- /dev/null
+++ b/wpimath/src/test/native/include/drake/math/autodiff.h
@@ -0,0 +1,332 @@
+/// @file
+/// Utilities for arithmetic on AutoDiffScalar.
+
+// TODO(russt): rename methods to be GSG compliant.
+
+#pragma once
+
+#include <cmath>
+#include <tuple>
+
+#include <Eigen/Core>
+
+#include "drake/common/autodiff.h"
+#include "drake/common/unused.h"
+
+namespace drake {
+namespace math {
+
+template <typename Derived>
+struct AutoDiffToValueMatrix {
+  typedef typename Eigen::Matrix<typename Derived::Scalar::Scalar,
+                                 Derived::RowsAtCompileTime,
+                                 Derived::ColsAtCompileTime> type;
+};
+
+template <typename Derived>
+typename AutoDiffToValueMatrix<Derived>::type autoDiffToValueMatrix(
+    const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
+  typename AutoDiffToValueMatrix<Derived>::type ret(auto_diff_matrix.rows(),
+                                                    auto_diff_matrix.cols());
+  for (int i = 0; i < auto_diff_matrix.rows(); i++) {
+    for (int j = 0; j < auto_diff_matrix.cols(); ++j) {
+      ret(i, j) = auto_diff_matrix(i, j).value();
+    }
+  }
+  return ret;
+}
+
+/** `B = DiscardGradient(A)` enables casting from a matrix of AutoDiffScalars
+ * to AutoDiffScalar::Scalar type, explicitly throwing away any gradient
+ * information. For a matrix of type, e.g. `MatrixX<AutoDiffXd> A`, the
+ * comparable operation
+ *   `B = A.cast<double>()`
+ * should (and does) fail to compile.  Use `DiscardGradient(A)` if you want to
+ * force the cast (and explicitly declare that information is lost).
+ *
+ * This method is overloaded to permit the user to call it for double types and
+ * AutoDiffScalar types (to avoid the calling function having to handle the
+ * two cases differently).
+ *
+ * @see DiscardZeroGradient
+ */
+template <typename Derived>
+typename std::enable_if<
+    !std::is_same<typename Derived::Scalar, double>::value,
+    Eigen::Matrix<typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime,
+                  Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime,
+                  Derived::MaxColsAtCompileTime>>::type
+DiscardGradient(const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
+  return autoDiffToValueMatrix(auto_diff_matrix);
+}
+
+/// @see DiscardGradient().
+template <typename Derived>
+typename std::enable_if<
+    std::is_same<typename Derived::Scalar, double>::value,
+    const Eigen::MatrixBase<Derived>&>::type
+DiscardGradient(const Eigen::MatrixBase<Derived>& matrix) {
+  return matrix;
+}
+
+/// @see DiscardGradient().
+template <typename _Scalar, int _Dim, int _Mode, int _Options>
+typename std::enable_if<
+    !std::is_same<_Scalar, double>::value,
+    Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>>::type
+DiscardGradient(const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&
+                    auto_diff_transform) {
+  return Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>(
+      autoDiffToValueMatrix(auto_diff_transform.matrix()));
+}
+
+/// @see DiscardGradient().
+template <typename _Scalar, int _Dim, int _Mode, int _Options>
+typename std::enable_if<std::is_same<_Scalar, double>::value,
+                        const Eigen::Transform<_Scalar, _Dim, _Mode,
+    _Options>&>::type
+DiscardGradient(
+    const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform) {
+  return transform;
+}
+
+
+/** \brief Initialize a single autodiff matrix given the corresponding value
+ *matrix.
+ *
+ * Set the values of \p auto_diff_matrix to be equal to \p val, and for each
+ *element i of \p auto_diff_matrix,
+ * resize the derivatives vector to \p num_derivatives, and set derivative
+ *number \p deriv_num_start + i to one (all other elements of the derivative
+ *vector set to zero).
+ *
+ * \param[in] mat 'regular' matrix of values
+ * \param[out] ret AutoDiff matrix
+ * \param[in] num_derivatives the size of the derivatives vector @default the
+ *size of mat
+ * \param[in] deriv_num_start starting index into derivative vector (i.e.
+ *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
+ *@default 0
+ */
+template <typename Derived, typename DerivedAutoDiff>
+void initializeAutoDiff(const Eigen::MatrixBase<Derived>& val,
+                        // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
+                        Eigen::MatrixBase<DerivedAutoDiff>& auto_diff_matrix,
+                        Eigen::DenseIndex num_derivatives = Eigen::Dynamic,
+                        Eigen::DenseIndex deriv_num_start = 0) {
+  using ADScalar = typename DerivedAutoDiff::Scalar;
+  static_assert(static_cast<int>(Derived::RowsAtCompileTime) ==
+                    static_cast<int>(DerivedAutoDiff::RowsAtCompileTime),
+                "auto diff matrix has wrong number of rows at compile time");
+  static_assert(static_cast<int>(Derived::ColsAtCompileTime) ==
+                    static_cast<int>(DerivedAutoDiff::ColsAtCompileTime),
+                "auto diff matrix has wrong number of columns at compile time");
+
+  if (num_derivatives == Eigen::Dynamic) num_derivatives = val.size();
+
+  auto_diff_matrix.resize(val.rows(), val.cols());
+  Eigen::DenseIndex deriv_num = deriv_num_start;
+  for (Eigen::DenseIndex i = 0; i < val.size(); i++) {
+    auto_diff_matrix(i) = ADScalar(val(i), num_derivatives, deriv_num++);
+  }
+}
+
+/** \brief The appropriate AutoDiffScalar gradient type given the value type and
+ * the number of derivatives at compile time
+ */
+template <typename Derived, int Nq>
+using AutoDiffMatrixType = Eigen::Matrix<
+    Eigen::AutoDiffScalar<Eigen::Matrix<typename Derived::Scalar, Nq, 1>>,
+    Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0,
+    Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>;
+
+/** \brief Initialize a single autodiff matrix given the corresponding value
+ *matrix.
+ *
+ * Create autodiff matrix that matches \p mat in size with derivatives of
+ *compile time size \p Nq and runtime size \p num_derivatives.
+ * Set its values to be equal to \p val, and for each element i of \p
+ *auto_diff_matrix, set derivative number \p deriv_num_start + i to one (all
+ *other derivatives set to zero).
+ *
+ * \param[in] mat 'regular' matrix of values
+ * \param[in] num_derivatives the size of the derivatives vector @default the
+ *size of mat
+ * \param[in] deriv_num_start starting index into derivative vector (i.e.
+ *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
+ *@default 0
+ * \return AutoDiff matrix
+ */
+template <int Nq = Eigen::Dynamic, typename Derived>
+AutoDiffMatrixType<Derived, Nq> initializeAutoDiff(
+    const Eigen::MatrixBase<Derived>& mat,
+    Eigen::DenseIndex num_derivatives = -1,
+    Eigen::DenseIndex deriv_num_start = 0) {
+  if (num_derivatives == -1) num_derivatives = mat.size();
+
+  AutoDiffMatrixType<Derived, Nq> ret(mat.rows(), mat.cols());
+  initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start);
+  return ret;
+}
+
+namespace internal {
+template <typename Derived, typename Scalar>
+struct ResizeDerivativesToMatchScalarImpl {
+  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
+  static void run(Eigen::MatrixBase<Derived>&, const Scalar&) {}
+};
+
+template <typename Derived, typename DerivType>
+struct ResizeDerivativesToMatchScalarImpl<Derived,
+                                          Eigen::AutoDiffScalar<DerivType>> {
+  using Scalar = Eigen::AutoDiffScalar<DerivType>;
+  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
+  static void run(Eigen::MatrixBase<Derived>& mat, const Scalar& scalar) {
+    for (int i = 0; i < mat.size(); i++) {
+      auto& derivs = mat(i).derivatives();
+      if (derivs.size() == 0) {
+        derivs.resize(scalar.derivatives().size());
+        derivs.setZero();
+      }
+    }
+  }
+};
+}  // namespace internal
+
+/** Resize derivatives vector of each element of a matrix to to match the size
+ * of the derivatives vector of a given scalar.
+ * \brief If the mat and scalar inputs are AutoDiffScalars, resize the
+ * derivatives vector of each element of the matrix mat to match
+ * the number of derivatives of the scalar. This is useful in functions that
+ * return matrices that do not depend on an AutoDiffScalar
+ * argument (e.g. a function with a constant output), while it is desired that
+ * information about the number of derivatives is preserved.
+ * \param mat matrix, for which the derivative vectors of the elements will be
+ * resized
+ * \param scalar scalar to match the derivative size vector against.
+ */
+template <typename Derived>
+// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
+void resizeDerivativesToMatchScalar(Eigen::MatrixBase<Derived>& mat,
+                                    const typename Derived::Scalar& scalar) {
+  internal::ResizeDerivativesToMatchScalarImpl<
+      Derived, typename Derived::Scalar>::run(mat, scalar);
+}
+
+namespace internal {
+/** \brief Helper for totalSizeAtCompileTime function (recursive)
+ */
+template <typename Head, typename... Tail>
+struct TotalSizeAtCompileTime {
+  static constexpr int eval() {
+    return Head::SizeAtCompileTime == Eigen::Dynamic ||
+                   TotalSizeAtCompileTime<Tail...>::eval() == Eigen::Dynamic
+               ? Eigen::Dynamic
+               : Head::SizeAtCompileTime +
+                     TotalSizeAtCompileTime<Tail...>::eval();
+  }
+};
+
+/** \brief Helper for totalSizeAtCompileTime function (base case)
+ */
+template <typename Head>
+struct TotalSizeAtCompileTime<Head> {
+  static constexpr int eval() { return Head::SizeAtCompileTime; }
+};
+
+/** \brief Determine the total size at compile time of a number of arguments
+ * based on their SizeAtCompileTime static members
+ */
+template <typename... Args>
+constexpr int totalSizeAtCompileTime() {
+  return TotalSizeAtCompileTime<Args...>::eval();
+}
+
+/** \brief Determine the total size at runtime of a number of arguments using
+ * their size() methods (base case).
+ */
+constexpr Eigen::DenseIndex totalSizeAtRunTime() { return 0; }
+
+/** \brief Determine the total size at runtime of a number of arguments using
+ * their size() methods (recursive)
+ */
+template <typename Head, typename... Tail>
+Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase<Head>& head,
+                                     const Tail&... tail) {
+  return head.size() + totalSizeAtRunTime(tail...);
+}
+
+/** \brief Helper for initializeAutoDiffTuple function (recursive)
+ */
+template <size_t Index>
+struct InitializeAutoDiffTupleHelper {
+  template <typename... ValueTypes, typename... AutoDiffTypes>
+  static void run(const std::tuple<ValueTypes...>& values,
+                  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
+                  std::tuple<AutoDiffTypes...>& auto_diffs,
+                  Eigen::DenseIndex num_derivatives,
+                  Eigen::DenseIndex deriv_num_start) {
+    constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index;
+    const auto& value = std::get<tuple_index>(values);
+    auto& auto_diff = std::get<tuple_index>(auto_diffs);
+    auto_diff.resize(value.rows(), value.cols());
+    initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start);
+    InitializeAutoDiffTupleHelper<Index - 1>::run(
+        values, auto_diffs, num_derivatives, deriv_num_start + value.size());
+  }
+};
+
+/** \brief Helper for initializeAutoDiffTuple function (base case)
+ */
+template <>
+struct InitializeAutoDiffTupleHelper<0> {
+  template <typename... ValueTypes, typename... AutoDiffTypes>
+  static void run(const std::tuple<ValueTypes...>& values,
+                  const std::tuple<AutoDiffTypes...>& auto_diffs,
+                  Eigen::DenseIndex num_derivatives,
+                  Eigen::DenseIndex deriv_num_start) {
+    unused(values, auto_diffs, num_derivatives, deriv_num_start);
+  }
+};
+}  // namespace internal
+
+/** \brief Given a series of Eigen matrices, create a tuple of corresponding
+ *AutoDiff matrices with values equal to the input matrices and properly
+ *initialized derivative vectors.
+ *
+ * The size of the derivative vector of each element of the matrices in the
+ *output tuple will be the same, and will equal the sum of the number of
+ *elements of the matrices in \p args.
+ * If all of the matrices in \p args have fixed size, then the derivative
+ *vectors will also have fixed size (being the sum of the sizes at compile time
+ *of all of the input arguments),
+ * otherwise the derivative vectors will have dynamic size.
+ * The 0th element of the derivative vectors will correspond to the derivative
+ *with respect to the 0th element of the first argument.
+ * Subsequent derivative vector elements correspond first to subsequent elements
+ *of the first input argument (traversed first by row, then by column), and so
+ *on for subsequent arguments.
+ *
+ * \param args a series of Eigen matrices
+ * \return a tuple of properly initialized AutoDiff matrices corresponding to \p
+ *args
+ *
+ */
+template <typename... Deriveds>
+std::tuple<AutoDiffMatrixType<
+    Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
+initializeAutoDiffTuple(const Eigen::MatrixBase<Deriveds>&... args) {
+  Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...);
+  std::tuple<AutoDiffMatrixType<
+      Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
+      ret(AutoDiffMatrixType<Deriveds,
+                             internal::totalSizeAtCompileTime<Deriveds...>()>(
+          args.rows(), args.cols())...);
+  auto values = std::forward_as_tuple(args...);
+  internal::InitializeAutoDiffTupleHelper<sizeof...(args)>::run(
+      values, ret, dynamic_num_derivs, 0);
+  return ret;
+}
+
+}  // namespace math
+}  // namespace drake

wpimath/src/test/native/include/trajectory/TestTrajectory.h

diff --git a/wpimath/src/test/native/include/trajectory/TestTrajectory.h b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
new file mode 100644
index 0000000..1cac87f
--- /dev/null
+++ b/wpimath/src/test/native/include/trajectory/TestTrajectory.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+class TestTrajectory {
+ public:
+  static Trajectory GetTrajectory(TrajectoryConfig& config) {
+    // 2018 cross scale auto waypoints
+    const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
+    const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
+
+    config.SetReversed(true);
+
+    auto vector = std::vector<Translation2d>{
+        (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
+            .Translation(),
+        (sideStart +
+         Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
+            .Translation()};
+
+    return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,
+                                                   crossScale, config);
+  }
+};
+
+}  // namespace frc

wpimath/wpimath-config.cmake.in

diff --git a/wpimath/wpimath-config.cmake.in b/wpimath/wpimath-config.cmake.in
new file mode 100644
index 0000000..2f661d9
--- /dev/null
+++ b/wpimath/wpimath-config.cmake.in
@@ -0,0 +1,5 @@
+include(CMakeFindDependencyMacro)
+@FILENAME_DEP_REPLACE@
+@WPIUTIL_DEP_REPLACE@
+
+include(${SELF_DIR}/wpimath.cmake)

wpiutil/.styleguide

diff --git a/wpiutil/.styleguide b/wpiutil/.styleguide
index 134c65d..2edcfaa 100644
--- a/wpiutil/.styleguide
+++ b/wpiutil/.styleguide
@@ -1,6 +1,7 @@
 cppHeaderFileInclude {
   \.h$
   \.inc$
+  \.inl$
 }
 
 cppSrcFileInclude {
@@ -10,10 +11,7 @@
 generatedFileExclude {
   src/main/native/cpp/http_parser\.cpp$
   src/main/native/cpp/llvm/
-  src/main/native/eigeninclude/
   src/main/native/include/llvm/
-  src/main/native/include/units/units\.h$
-  src/main/native/include/unsupported/
   src/main/native/include/wpi/AlignOf\.h$
   src/main/native/include/wpi/ArrayRef\.h$
   src/main/native/include/wpi/Chrono\.h$
@@ -68,7 +66,6 @@
   src/main/native/include/uv/
   src/main/native/libuv/
   src/main/native/resources/
-  src/test/native/cpp/UnitsTest\.cpp$
   src/test/native/cpp/llvm/
   src/main/native/windows/StackWalker
 }

wpiutil/BUILD

diff --git a/wpiutil/BUILD b/wpiutil/BUILD
deleted file mode 100644
index 98ed173..0000000
--- a/wpiutil/BUILD
+++ /dev/null
@@ -1,28 +0,0 @@
-licenses(["notice"])
-
-cc_library(
-    name = "wpiutil",
-    srcs = glob(
-        [
-            "src/main/native/cpp/llvm/*.cpp",
-        ],
-    ) + [
-        "src/main/native/cpp/llvm/Unix/Path.inc",
-        "src/main/native/cpp/timestamp.cpp",
-        "src/main/native/cpp/SafeThread.cpp",
-    ],
-    hdrs = glob(
-        [
-            "src/main/native/include/**",
-        ],
-        exclude = ["STLExtras.h"],
-    ),
-    copts = [
-        "-Wno-unused-parameter",
-    ],
-    includes = [
-        "src/main/native/include",
-    ],
-    restricted_to = ["//tools:roborio"],
-    visibility = ["//visibility:public"],
-)

wpiutil/CMakeLists.txt

diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt
index 559c8ed..f500c89 100644
--- a/wpiutil/CMakeLists.txt
+++ b/wpiutil/CMakeLists.txt
@@ -8,36 +8,12 @@
 file(GLOB wpiutil_jni_src src/main/native/cpp/jni/WPIUtilJNI.cpp)
 
 # Java bindings
-if (NOT WITHOUT_JAVA)
+if (WITH_JAVA)
   find_package(Java REQUIRED)
   find_package(JNI REQUIRED)
   include(UseJava)
   set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
 
-  if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/ejml-simple-0.38.jar")
-      set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
-      set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml")
-
-      message(STATUS "Downloading EJML jarfiles...")
-
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-cdense/0.38/ejml-cdense-0.38.jar"
-          "${JAR_ROOT}/ejml-cdense-0.38.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-core/0.38/ejml-core-0.38.jar"
-          "${JAR_ROOT}/ejml-core-0.38.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-ddense/0.38/ejml-ddense-0.38.jar"
-          "${JAR_ROOT}/ejml-ddense-0.38.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-dsparse/0.38/ejml-dsparse-0.38.jar"
-          "${JAR_ROOT}/ejml-dsparse-0.38.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-fdense/0.38/ejml-fdense-0.38.jar"
-          "${JAR_ROOT}/ejml-fdense-0.38.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-simple/0.38/ejml-simple-0.38.jar"
-          "${JAR_ROOT}/ejml-simple-0.38.jar")
-      file(DOWNLOAD "${BASE_URL}org/ejml/ejml-zdense/0.38/ejml-zdense-0.38.jar"
-          "${JAR_ROOT}/ejml-zdense-0.38.jar")
-
-      message(STATUS "All files downloaded.")
-  endif()
-
   if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.10.0.jar")
         set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
         set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson")
@@ -54,28 +30,20 @@
         message(STATUS "All files downloaded.")
     endif()
 
-  file(GLOB EJML_JARS
-      ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar)
-
   file(GLOB JACKSON_JARS
         ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar)
 
-  set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${EJML_JARS})
-
-  execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpiutil/generate_numbers.py ${CMAKE_BINARY_DIR}/wpiutil RESULT_VARIABLE generateResult)
-  if(NOT (generateResult EQUAL "0"))
-    message(FATAL_ERROR "python3 generate_numbers.py failed")
-  endif()
+  set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${JACKSON_JARS})
 
   set(CMAKE_JNI_TARGET true)
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${CMAKE_BINARY_DIR}/wpiutil/generated/*.java)
+  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
 
   if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
     set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
-    add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil)
+    add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${JACKSON_JARS} OUTPUT_NAME wpiutil)
   else()
-    add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers)
+    add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${JACKSON_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers)
   endif()
 
   get_property(WPIUTIL_JAR_FILE TARGET wpiutil_jar PROPERTY JAR_FILE)
@@ -171,22 +139,12 @@
 
 target_compile_features(wpiutil PUBLIC cxx_std_17)
 if (MSVC)
-    target_compile_options(wpiutil PUBLIC /permissive- /Zc:throwingNew)
+    target_compile_options(wpiutil PUBLIC /permissive- /Zc:throwingNew /MP /bigobj)
     target_compile_definitions(wpiutil PRIVATE -D_CRT_SECURE_NO_WARNINGS)
 endif()
 wpilib_target_warnings(wpiutil)
 target_link_libraries(wpiutil Threads::Threads ${CMAKE_DL_LIBS} ${ATOMIC})
 
-if (NOT USE_VCPKG_EIGEN)
-    install(DIRECTORY src/main/native/eigeninclude/ DESTINATION "${include_dest}/wpiutil")
-    target_include_directories(wpiutil PUBLIC
-                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/eigeninclude>
-                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
-else()
-    find_package(Eigen3 CONFIG REQUIRED)
-    target_link_libraries (wpiutil Eigen3::Eigen)
-endif()
-
 if (NOT USE_VCPKG_LIBUV)
     target_sources(wpiutil PRIVATE ${uv_native_src})
     install(DIRECTORY src/main/native/libuv/include/ DESTINATION "${include_dest}/wpiutil")
@@ -227,11 +185,11 @@
 install(TARGETS wpiutil EXPORT wpiutil DESTINATION "${main_lib_dest}")
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpiutil")
 
-if (NOT WITHOUT_JAVA AND MSVC)
+if (WITH_JAVA AND MSVC)
     install(TARGETS wpiutil RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
 endif()
 
-if (MSVC OR FLAT_INSTALL_WPILIB)
+if (WITH_FLAT_INSTALL)
     set (wpiutil_config_dir ${wpilib_dest})
 else()
     set (wpiutil_config_dir share/wpiutil)
@@ -273,5 +231,6 @@
 
 if (WITH_TESTS)
     wpilib_add_test(wpiutil src/test/native/cpp)
+    target_include_directories(wpiutil_test PRIVATE src/test/native/include)
     target_link_libraries(wpiutil_test wpiutil ${LIBUTIL} gmock_main)
 endif()

wpiutil/build.gradle

diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle
index a79bae4..f2efe28 100644
--- a/wpiutil/build.gradle
+++ b/wpiutil/build.gradle
@@ -2,6 +2,7 @@
 
 ext {
     noWpiutil = true
+    skipJniCheck = true
     baseId = 'wpiutil'
     groupId = 'edu.wpi.first.wpiutil'
 
@@ -172,9 +173,6 @@
     from('src/main/native/libuv/include') {
         into '/'
     }
-    from('src/main/native/eigeninclude') {
-        into '/'
-    }
 }
 
 model {
@@ -182,7 +180,7 @@
         all {
             it.sources.each {
                 it.exportedHeaders {
-                    srcDirs 'src/main/native/include', 'src/main/native/libuv/include', 'src/main/native/libuv/src', 'src/main/native/eigeninclude'
+                    srcDirs 'src/main/native/include', 'src/main/native/libuv/include', 'src/main/native/libuv/src'
                 }
             }
         }
@@ -242,68 +240,7 @@
 }
 
 dependencies {
-    api "org.ejml:ejml-simple:0.38"
     api "com.fasterxml.jackson.core:jackson-annotations:2.10.0"
     api "com.fasterxml.jackson.core:jackson-core:2.10.0"
     api "com.fasterxml.jackson.core:jackson-databind:2.10.0"
 }
-
-def wpilibNumberFileInput = file("src/generate/GenericNumber.java.in")
-def natFileInput = file("src/generate/Nat.java.in")
-def natGetterInput = file("src/generate/NatGetter.java.in")
-def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/numbers")
-def wpilibNatFileOutput = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/Nat.java")
-def maxNum = 20
-
-task generateNumbers() {
-    description = "Generates generic number classes from template"
-    group = "WPILib"
-
-    inputs.file wpilibNumberFileInput
-    outputs.dir wpilibNumberFileOutputDir
-
-    doLast {
-        if(wpilibNumberFileOutputDir.exists()) {
-            wpilibNumberFileOutputDir.delete()
-        }
-        wpilibNumberFileOutputDir.mkdirs()
-
-        for(i in 0..maxNum) {
-            def outputFile = new File(wpilibNumberFileOutputDir, "N${i}.java")
-            def read = wpilibNumberFileInput.text.replace('${num}', i.toString())
-            outputFile.write(read)
-        }
-    }
-}
-
-task generateNat() {
-    description = "Generates Nat.java"
-    group = "WPILib"
-    inputs.files([natFileInput, natGetterInput])
-    outputs.file wpilibNatFileOutput
-    dependsOn generateNumbers
-
-    doLast {
-        if(wpilibNatFileOutput.exists()) {
-            wpilibNatFileOutput.delete()
-        }
-
-        def template = natFileInput.text + "\n"
-
-        def importsString = "";
-
-        for(i in 0..maxNum) {
-            importsString += "import edu.wpi.first.wpiutil.math.numbers.N${i};\n"
-            template += natGetterInput.text.replace('${num}', i.toString()) + "\n"
-        }
-        template += "}\n" // Close the class body
-
-        template = template.replace('{{REPLACEWITHIMPORTS}}', importsString)
-
-        wpilibNatFileOutput.write(template)
-    }
-}
-
-sourceSets.main.java.srcDir "${buildDir}/generated/java"
-compileJava.dependsOn generateNumbers
-compileJava.dependsOn generateNat

wpiutil/generate_numbers.py

diff --git a/wpiutil/generate_numbers.py b/wpiutil/generate_numbers.py
deleted file mode 100644
index 61cc02a..0000000
--- a/wpiutil/generate_numbers.py
+++ /dev/null
@@ -1,42 +0,0 @@
-import os
-import shutil
-import sys
-
-MAX_NUM = 20
-
-dirname, _ = os.path.split(os.path.abspath(__file__))
-cmake_binary_dir = sys.argv[1]
-
-with open(f"{dirname}/src/generate/GenericNumber.java.in", "r") as templateFile:
-    template = templateFile.read()
-    rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/numbers"
-
-    if os.path.exists(rootPath):
-        shutil.rmtree(rootPath)
-    os.makedirs(rootPath)
-
-    for i in range(MAX_NUM + 1):
-        with open(f"{rootPath}/N{i}.java", "w") as f:
-            f.write(template.replace("${num}", str(i)))
-
-with open(f"{dirname}/src/generate/Nat.java.in", "r") as templateFile:
-    template = templateFile.read()
-    outputPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/Nat.java"
-    with open(f"{dirname}/src/generate/NatGetter.java.in", "r") as getterFile:
-        getter = getterFile.read()
-
-    if os.path.exists(outputPath):
-        os.remove(outputPath)
-
-    importsString = ""
-
-    for i in range(MAX_NUM + 1):
-        importsString += f"import edu.wpi.first.wpiutil.math.numbers.N{i};\n"
-        template += getter.replace("${num}", str(i))
-
-    template += "}\n"
-
-    template = template.replace('{{REPLACEWITHIMPORTS}}', importsString)
-
-    with open(outputPath, "w") as f:
-        f.write(template)

wpiutil/src/generate/Nat.java.in

diff --git a/wpiutil/src/generate/Nat.java.in b/wpiutil/src/generate/Nat.java.in
deleted file mode 100644
index 8c471f6..0000000
--- a/wpiutil/src/generate/Nat.java.in
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-//CHECKSTYLE.OFF: ImportOrder
-{{REPLACEWITHIMPORTS}}
-//CHECKSTYLE.ON
-
-/**
- * A natural number expressed as a java class.
- * The counterpart to {@link Num} that should be used as a concrete value.
- *
- * @param <T> The {@link Num} this represents.
- */
-@SuppressWarnings({"MethodName", "unused", "PMD.TooManyMethods"})
-public interface Nat<T extends Num> {
-  /**
-   * The number this interface represents.
-   *
-   * @return The number backing this value.
-   */
-  int getNum();

wpiutil/src/main/java/edu/wpi/first/wpiutil/CombinedRuntimeLoader.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/CombinedRuntimeLoader.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/CombinedRuntimeLoader.java
new file mode 100644
index 0000000..8f0d53e
--- /dev/null
+++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/CombinedRuntimeLoader.java
@@ -0,0 +1,184 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil;
+
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.type.TypeReference;
+import com.fasterxml.jackson.databind.ObjectMapper;
+
+public final class CombinedRuntimeLoader {
+  private CombinedRuntimeLoader() {
+  }
+
+  private static String extractionDirectory;
+
+  public static synchronized String getExtractionDirectory() {
+    return extractionDirectory;
+  }
+
+  private static synchronized void setExtractionDirectory(String directory) {
+    extractionDirectory = directory;
+  }
+
+  public static native String setDllDirectory(String directory);
+
+  private static String getLoadErrorMessage(String libraryName, UnsatisfiedLinkError ule) {
+    StringBuilder msg = new StringBuilder(512);
+    msg.append(libraryName).append(" could not be loaded from path\n"
+        + "\tattempted to load for platform ")
+        .append(RuntimeDetector.getPlatformPath())
+        .append("\nLast Load Error: \n")
+        .append(ule.getMessage())
+        .append('\n');
+    if (RuntimeDetector.isWindows()) {
+      msg.append("A common cause of this error is missing the C++ runtime.\n"
+          + "Download the latest at https://support.microsoft.com/en-us/help/2977003/the-latest-supported-visual-c-downloads\n");
+    }
+    return msg.toString();
+  }
+
+  /**
+   * Extract a list of native libraries.
+   * @param <T>             The class where the resources would be located
+   * @param clazz           The actual class object
+   * @param resourceName    The resource name on the classpath to use for file lookup
+   * @return                List of all libraries that were extracted
+   * @throws IOException    Thrown if resource not found or file could not be extracted
+   */
+  @SuppressWarnings({"PMD.AvoidInstantiatingObjectsInLoops", "PMD.UnnecessaryCastRule"})
+  public static <T> List<String> extractLibraries(Class<T> clazz, String resourceName)
+      throws IOException {
+    TypeReference<HashMap<String, Object>> typeRef = new TypeReference<HashMap<String, Object>>() {
+    };
+    ObjectMapper mapper = new ObjectMapper();
+    Map<String, Object> map;
+    try (var stream = clazz.getResourceAsStream(resourceName)) {
+      map = mapper.readValue(stream, typeRef);
+    }
+
+    var platformPath = Paths.get(RuntimeDetector.getPlatformPath());
+    var platform = platformPath.getName(0).toString();
+    var arch = platformPath.getName(1).toString();
+
+    var platformMap = (Map<String, List<String>>) map.get(platform);
+
+    var fileList = platformMap.get(arch);
+
+    var extractionPathString = getExtractionDirectory();
+
+    if (extractionPathString == null) {
+      String hash = (String) map.get("hash");
+
+      var defaultExtractionRoot = RuntimeLoader.getDefaultExtractionRoot();
+      var extractionPath = Paths.get(defaultExtractionRoot, platform, arch, hash);
+      extractionPathString = extractionPath.toString();
+
+      setExtractionDirectory(extractionPathString);
+    }
+
+    List<String> extractedFiles = new ArrayList<>();
+
+    byte[] buffer = new byte[0x10000]; // 64K copy buffer
+
+    for (var file : fileList) {
+      try (var stream = clazz.getResourceAsStream(file)) {
+        Objects.requireNonNull(stream);
+
+        var outputFile = Paths.get(extractionPathString, new File(file).getName());
+        extractedFiles.add(outputFile.toString());
+        if (outputFile.toFile().exists()) {
+          continue;
+        }
+        outputFile.getParent().toFile().mkdirs();
+
+        try (var os = Files.newOutputStream(outputFile)) {
+          int readBytes;
+          while ((readBytes = stream.read(buffer)) != -1) { // NOPMD
+            os.write(buffer, 0, readBytes);
+          }
+        }
+      }
+    }
+
+    return extractedFiles;
+  }
+
+  /**
+   * Load a single library from a list of extracted files.
+   * @param libraryName The library name to load
+   * @param extractedFiles The extracted files to search
+   * @throws IOException If library was not found
+   */
+  public static void loadLibrary(String libraryName, List<String> extractedFiles)
+      throws IOException {
+    String currentPath = null;
+    String oldDllDirectory = null;
+    try {
+      if (RuntimeDetector.isWindows()) {
+        var extractionPathString = getExtractionDirectory();
+        oldDllDirectory = setDllDirectory(extractionPathString);
+      }
+      for (var extractedFile : extractedFiles) {
+        if (extractedFile.contains(libraryName)) {
+          // Load it
+          currentPath = extractedFile;
+          System.load(extractedFile);
+          return;
+        }
+      }
+      throw new IOException("Could not find library " + libraryName);
+    } catch (UnsatisfiedLinkError ule) {
+      throw new IOException(getLoadErrorMessage(currentPath, ule));
+    } finally {
+      if (oldDllDirectory != null) {
+        setDllDirectory(oldDllDirectory);
+      }
+    }
+  }
+
+  /**
+   * Load a list of native libraries out of a single directory.
+   *
+   * @param <T>             The class where the resources would be located
+   * @param clazz           The actual class object
+   * @param librariesToLoad List of libraries to load
+   * @throws IOException Throws an IOException if not found
+   */
+  public static <T> void loadLibraries(Class<T> clazz, String... librariesToLoad)
+      throws IOException {
+    // Extract everything
+
+    var extractedFiles = extractLibraries(clazz, "/ResourceInformation.json");
+
+    String currentPath = "";
+
+    try {
+      if (RuntimeDetector.isWindows()) {
+        var extractionPathString = getExtractionDirectory();
+        // Load windows, set dll directory
+        currentPath = Paths.get(extractionPathString, "WindowsLoaderHelper.dll").toString();
+        System.load(currentPath);
+      }
+    } catch (UnsatisfiedLinkError ule) {
+      throw new IOException(getLoadErrorMessage(currentPath, ule));
+    }
+
+    for (var library : librariesToLoad) {
+      loadLibrary(library, extractedFiles);
+    }
+  }
+}

wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java
new file mode 100644
index 0000000..70387eb
--- /dev/null
+++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil;
+
+import static java.util.Objects.requireNonNull;
+
+/**
+ * Utility class for common WPILib error messages.
+ */
+public final class ErrorMessages {
+  /**
+   * Utility class, so constructor is private.
+   */
+  private ErrorMessages() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Requires that a parameter of a method not be null; prints an error message with
+   * helpful debugging instructions if the parameter is null.
+   *
+   * @param obj The parameter that must not be null.
+   * @param paramName The name of the parameter.
+   * @param methodName The name of the method.
+   */
+  public static <T> T requireNonNullParam(T obj, String paramName, String methodName) {
+    return requireNonNull(obj,
+        "Parameter " + paramName + " in method " + methodName + " was null when it"
+            + " should not have been!  Check the stacktrace to find the responsible line of code - "
+            + "usually, it is the first line of user-written code indicated in the stacktrace.  "
+            + "Make sure all objects passed to the method in question were properly initialized -"
+            + " note that this may not be obvious if it is being called under "
+            + "dynamically-changing conditions!  Please do not seek additional technical assistance"
+            + " without doing this first!");
+  }
+}

wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java
index f7379dd..b4c07b7 100644
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java
+++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -55,7 +55,7 @@
         filePath = "/linux/athena/";
       } else if (isRaspbian()) {
         filePath = "/linux/raspbian/";
-      } else if (isAarch64Bionic()) {
+      } else if (isAarch64()) {
         filePath = "/linux/aarch64bionic/";
       } else {
         filePath = "/linux/nativearm/";
@@ -128,28 +128,12 @@
     }
   }
 
-  /** check if os is bionic aarch64.
+  /** check if architecture is aarch64.
    *
-   * @return if os is bionic aarch64
+   * @return if architecture is aarch64
    */
-  public static boolean isAarch64Bionic() {
-    if (!System.getProperty("os.arch").equals("aarch64")) {
-      return false;
-    }
-    try (BufferedReader reader = Files.newBufferedReader(Paths.get("/etc/os-release"))) {
-      String value = reader.readLine();
-      String version = "";
-      while (value != null) {
-        if (value.contains("VERSION=")) {
-          version = value;
-          break;
-        }
-        value = reader.readLine();
-      }
-      return version.contains("Bionic");
-    } catch (IOException ex) {
-      return false;
-    }
+  public static boolean isAarch64() {
+    return System.getProperty("os.arch").equals("aarch64");
   }
 
   public static boolean isLinux() {

wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java
index 3e5fea7..21193a2 100644
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java
+++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -51,13 +51,19 @@
     m_extractionRoot = extractionRoot;
   }
 
-  private String getLoadErrorMessage() {
-    StringBuilder msg = new StringBuilder(256);
+  private String getLoadErrorMessage(UnsatisfiedLinkError ule) {
+    StringBuilder msg = new StringBuilder(512);
     msg.append(m_libraryName)
        .append(" could not be loaded from path or an embedded resource.\n"
                + "\tattempted to load for platform ")
        .append(RuntimeDetector.getPlatformPath())
+       .append("\nLast Load Error: \n")
+       .append(ule.getMessage())
        .append('\n');
+    if (RuntimeDetector.isWindows()) {
+      msg.append("A common cause of this error is missing the C++ runtime.\n"
+                 + "Download the latest at https://support.microsoft.com/en-us/help/2977003/the-latest-supported-visual-c-downloads\n");
+    }
     return msg.toString();
   }
 
@@ -75,7 +81,7 @@
       String resname = RuntimeDetector.getLibraryResource(m_libraryName);
       try (InputStream hashIs = m_loadClass.getResourceAsStream(hashName)) {
         if (hashIs == null) {
-          throw new IOException(getLoadErrorMessage());
+          throw new IOException(getLoadErrorMessage(ule));
         }
         try (Scanner scanner = new Scanner(hashIs, StandardCharsets.UTF_8.name())) {
           String hash = scanner.nextLine();
@@ -87,7 +93,7 @@
             // If extraction failed, extract
             try (InputStream resIs = m_loadClass.getResourceAsStream(resname)) {
               if (resIs == null) {
-                throw new IOException(getLoadErrorMessage());
+                throw new IOException(getLoadErrorMessage(ule));
               }
               jniLibrary.getParentFile().mkdirs();
               try (OutputStream os = Files.newOutputStream(jniLibrary.toPath())) {
@@ -120,7 +126,7 @@
       String hash = null;
       try (InputStream is = m_loadClass.getResourceAsStream(resname)) {
         if (is == null) {
-          throw new IOException(getLoadErrorMessage());
+          throw new IOException(getLoadErrorMessage(ule));
         }
         MessageDigest md = null;
         try {
@@ -152,7 +158,7 @@
         // If extraction failed, extract
         try (InputStream resIs = m_loadClass.getResourceAsStream(resname)) {
           if (resIs == null) {
-            throw new IOException(getLoadErrorMessage());
+            throw new IOException(getLoadErrorMessage(ule));
           }
           jniLibrary.getParentFile().mkdirs();
           try (OutputStream os = Files.newOutputStream(jniLibrary.toPath())) {

wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java
index 941e691..3a22c7b 100644
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java
+++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -51,6 +51,8 @@
     libraryLoaded = true;
   }
 
+  public static native long now();
+
   public static native void addPortForwarder(int port, String remoteHost, int remotePort);
   public static native void removePortForwarder(int port);
 }

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
deleted file mode 100644
index a5490a3..0000000
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
+++ /dev/null
@@ -1,50 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.Objects;
-
-import org.ejml.simple.SimpleMatrix;
-
-/**
- * A class for constructing arbitrary RxC matrices.
- *
- * @param <R> The number of rows of the desired matrix.
- * @param <C> The number of columns of the desired matrix.
- */
-public class MatBuilder<R extends Num, C extends Num> {
-  private final Nat<R> m_rows;
-  private final Nat<C> m_cols;
-
-  /**
-   * Fills the matrix with the given data, encoded in row major form.
-   * (The matrix is filled row by row, left to right with the given data).
-   *
-   * @param data The data to fill the matrix with.
-   * @return The constructed matrix.
-   */
-  @SuppressWarnings("LineLength")
-  public final Matrix<R, C> fill(double... data) {
-    if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
-      throw new IllegalArgumentException("Invalid matrix data provided. Wanted " + this.m_rows.getNum()
-          + " x " + this.m_cols.getNum() + " matrix, but got " + data.length + " elements");
-    } else {
-      return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
-    }
-  }
-
-  /**
-   * Creates a new {@link MatBuilder} with the given dimensions.
-   * @param rows The number of rows of the matrix.
-   * @param cols The number of columns of the matrix.
-   */
-  public MatBuilder(Nat<R> rows, Nat<C> cols) {
-    this.m_rows = Objects.requireNonNull(rows);
-    this.m_cols = Objects.requireNonNull(cols);
-  }
-}

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
deleted file mode 100644
index bd555dc..0000000
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-public final class MathUtil {
-  private MathUtil() {
-    throw new AssertionError("utility class");
-  }
-
-  /**
-   * Returns value clamped between low and high boundaries.
-   *
-   * @param value Value to clamp.
-   * @param low   The lower boundary to which to clamp value.
-   * @param high  The higher boundary to which to clamp value.
-   */
-  public static int clamp(int value, int low, int high) {
-    return Math.max(low, Math.min(value, high));
-  }
-
-  /**
-   * Returns value clamped between low and high boundaries.
-   *
-   * @param value Value to clamp.
-   * @param low   The lower boundary to which to clamp value.
-   * @param high  The higher boundary to which to clamp value.
-   */
-  public static double clamp(double value, double low, double high) {
-    return Math.max(low, Math.min(value, high));
-  }
-}

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
deleted file mode 100644
index 5780498..0000000
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
+++ /dev/null
@@ -1,327 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.Objects;
-
-import org.ejml.dense.row.CommonOps_DDRM;
-import org.ejml.dense.row.NormOps_DDRM;
-import org.ejml.simple.SimpleMatrix;
-
-/**
- * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
- *
- * <p>This class is intended to be used alongside the state space library.
- *
- * @param <R> The number of rows in this matrix.
- * @param <C> The number of columns in this matrix.
- */
-@SuppressWarnings("PMD.TooManyMethods")
-public class Matrix<R extends Num, C extends Num> {
-
-  private final SimpleMatrix m_storage;
-
-  /**
-   * Gets the number of columns in this matrix.
-   *
-   * @return The number of columns, according to the internal storage.
-   */
-  public final int getNumCols() {
-    return this.m_storage.numCols();
-  }
-
-  /**
-   * Gets the number of rows in this matrix.
-   *
-   * @return The number of rows, according to the internal storage.
-   */
-  public final int getNumRows() {
-    return this.m_storage.numRows();
-  }
-
-  /**
-   * Get an element of this matrix.
-   *
-   * @param row The row of the element.
-   * @param col The column of the element.
-   * @return The element in this matrix at row,col.
-   */
-  public final double get(int row, int col) {
-    return this.m_storage.get(row, col);
-  }
-
-  /**
-   * Sets the value at the given indices.
-   *
-   * @param row     The row of the element.
-   * @param col     The column of the element.
-   * @param value The value to insert at the given location.
-   */
-  public final void set(int row, int col, double value) {
-    this.m_storage.set(row, col, value);
-  }
-
-  /**
-   * If a vector then a square matrix is returned
-   * if a matrix then a vector of diagonal elements is returned.
-   *
-   * @return Diagonal elements inside a vector or a square matrix with the same diagonal elements.
-   */
-  public final Matrix<R, C> diag() {
-    return new Matrix<>(this.m_storage.diag());
-  }
-
-  /**
-   * Returns the largest element of this matrix.
-   *
-   * @return The largest element of this matrix.
-   */
-  public final double maxInternal() {
-    return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
-  }
-
-  /**
-   * Returns the smallest element of this matrix.
-   *
-   * @return The smallest element of this matrix.
-   */
-  public final double minInternal() {
-    return CommonOps_DDRM.elementMin(this.m_storage.getDDRM());
-  }
-
-  /**
-   * Calculates the mean of the elements in this matrix.
-   *
-   * @return The mean value of this matrix.
-   */
-  public final double mean() {
-    return this.elementSum() / (double) this.m_storage.getNumElements();
-  }
-
-  /**
-   * Multiplies this matrix with another that has C rows.
-   *
-   * <p>As matrix multiplication is only defined if the number of columns
-   * in the first matrix matches the number of rows in the second,
-   * this operation will fail to compile under any other circumstances.
-   *
-   * @param other The other matrix to multiply by.
-   * @param <C2>  The number of columns in the second matrix.
-   * @return The result of the matrix multiplication between this and the given matrix.
-   */
-  public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
-    return new Matrix<>(this.m_storage.mult(other.m_storage));
-  }
-
-  /**
-   * Multiplies all the elements of this matrix by the given scalar.
-   *
-   * @param value The scalar value to multiply by.
-   * @return A new matrix with all the elements multiplied by the given value.
-   */
-  public final Matrix<R, C> times(double value) {
-    return new Matrix<>(this.m_storage.scale(value));
-  }
-
-  /**
-   * <p>
-   * Returns a matrix which is the result of an element by element multiplication of 'this' and 'b'.
-   * c<sub>i,j</sub> = a<sub>i,j</sub>*b<sub>i,j</sub>
-   * </p>
-   *
-   * @param other A matrix.
-   * @return The element by element multiplication of 'this' and 'b'.
-   */
-  public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
-    return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
-  }
-
-  /**
-   * Subtracts the given value from all the elements of this matrix.
-   *
-   * @param value The value to subtract.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> minus(double value) {
-    return new Matrix<>(this.m_storage.minus(value));
-  }
-
-
-  /**
-   * Subtracts the given matrix from this matrix.
-   *
-   * @param value The matrix to subtract.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> minus(Matrix<R, C> value) {
-    return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
-  }
-
-
-  /**
-   * Adds the given value to all the elements of this matrix.
-   *
-   * @param value The value to add.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> plus(double value) {
-    return new Matrix<>(this.m_storage.plus(value));
-  }
-
-  /**
-   * Adds the given matrix to this matrix.
-   *
-   * @param value The matrix to add.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> plus(Matrix<R, C> value) {
-    return new Matrix<>(this.m_storage.plus(value.m_storage));
-  }
-
-  /**
-   * Divides all elements of this matrix by the given value.
-   *
-   * @param value The value to divide by.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> div(int value) {
-    return new Matrix<>(this.m_storage.divide((double) value));
-  }
-
-  /**
-   * Divides all elements of this matrix by the given value.
-   *
-   * @param value The value to divide by.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> div(double value) {
-    return new Matrix<>(this.m_storage.divide(value));
-  }
-
-  /**
-   * Calculates the transpose, M^T of this matrix.
-   *
-   * @return The tranpose matrix.
-   */
-  public final Matrix<C, R> transpose() {
-    return new Matrix<>(this.m_storage.transpose());
-  }
-
-
-  /**
-   * Returns a copy of this matrix.
-   *
-   * @return A copy of this matrix.
-   */
-  public final Matrix<R, C> copy() {
-    return new Matrix<>(this.m_storage.copy());
-  }
-
-
-  /**
-   * Returns the inverse matrix of this matrix.
-   *
-   * @return The inverse of this matrix.
-   * @throws org.ejml.data.SingularMatrixException If this matrix is non-invertable.
-   */
-  public final Matrix<R, C> inv() {
-    return new Matrix<>(this.m_storage.invert());
-  }
-
-  /**
-   * Returns the determinant of this matrix.
-   *
-   * @return The determinant of this matrix.
-   */
-  public final double det() {
-    return this.m_storage.determinant();
-  }
-
-  /**
-   * Computes the Frobenius normal of the matrix.<br>
-   * <br>
-   * normF = Sqrt{  &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>}   }
-   *
-   * @return The matrix's Frobenius normal.
-   */
-  public final double normF() {
-    return this.m_storage.normF();
-  }
-
-  /**
-   * Computes the induced p = 1 matrix norm.<br>
-   * <br>
-   * ||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
-   *
-   * @return The norm.
-   */
-  public final double normIndP1() {
-    return NormOps_DDRM.inducedP1(this.m_storage.getDDRM());
-  }
-
-  /**
-   * Computes the sum of all the elements in the matrix.
-   *
-   * @return Sum of all the elements.
-   */
-  public final double elementSum() {
-    return this.m_storage.elementSum();
-  }
-
-  /**
-   * Computes the trace of the matrix.
-   *
-   * @return The trace of the matrix.
-   */
-  public final double trace() {
-    return this.m_storage.trace();
-  }
-
-  /**
-   * Returns a matrix which is the result of an element by element power of 'this' and 'b':
-   * c<sub>i,j</sub> = a<sub>i,j</sub> ^ b.
-   *
-   * @param b Scalar
-   * @return The element by element power of 'this' and 'b'.
-   */
-  @SuppressWarnings("ParameterName")
-  public final Matrix<R, C> epow(double b) {
-    return new Matrix<>(this.m_storage.elementPower(b));
-  }
-
-  /**
-   * Returns a matrix which is the result of an element by element power of 'this' and 'b':
-   * c<sub>i,j</sub> = a<sub>i,j</sub> ^ b.
-   *
-   * @param b Scalar.
-   * @return The element by element power of 'this' and 'b'.
-   */
-  @SuppressWarnings("ParameterName")
-  public final Matrix<R, C> epow(int b) {
-    return new Matrix<>(this.m_storage.elementPower((double) b));
-  }
-
-  /**
-   * Returns the EJML {@link SimpleMatrix} backing this wrapper.
-   *
-   * @return The untyped EJML {@link SimpleMatrix}.
-   */
-  public final SimpleMatrix getStorage() {
-    return this.m_storage;
-  }
-
-  /**
-   * Constructs a new matrix with the given storage.
-   * Caller should make sure that the provided generic bounds match the shape of the provided matrix
-   *
-   * @param storage The {@link SimpleMatrix} to back this value
-   */
-  public Matrix(SimpleMatrix storage) {
-    this.m_storage = Objects.requireNonNull(storage);
-  }
-}

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
deleted file mode 100644
index ac4deb9..0000000
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
+++ /dev/null
@@ -1,83 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.Objects;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-public final class MatrixUtils {
-  private MatrixUtils() {
-    throw new AssertionError("utility class");
-  }
-
-  /**
-   * Creates a new matrix of zeros.
-   *
-   * @param rows The number of rows in the matrix.
-   * @param cols The number of columns in the matrix.
-   * @param <R> The number of rows in the matrix as a generic.
-   * @param <C> The number of columns in the matrix as a generic.
-   * @return An RxC matrix filled with zeros.
-   */
-  @SuppressWarnings("LineLength")
-  public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
-    return new Matrix<>(
-        new SimpleMatrix(Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
-  }
-
-  /**
-   * Creates a new vector of zeros.
-   *
-   * @param nums The size of the desired vector.
-   * @param <N> The size of the desired vector as a generic.
-   * @return A vector of size N filled with zeros.
-   */
-  public static <N extends Num> Matrix<N, N1> zeros(Nat<N> nums) {
-    return new Matrix<>(new SimpleMatrix(Objects.requireNonNull(nums).getNum(), 1));
-  }
-
-  /**
-   * Creates the identity matrix of the given dimension.
-   *
-   * @param dim The dimension of the desired matrix.
-   * @param <D> The dimension of the desired matrix as a generic.
-   * @return The DxD identity matrix.
-   */
-  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
-    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
-  }
-
-  /**
-   * Entrypoint to the MatBuilder class for creation
-   * of custom matrices with the given dimensions and contents.
-   *
-   * @param rows The number of rows of the desired matrix.
-   * @param cols The number of columns of the desired matrix.
-   * @param <R> The number of rows of the desired matrix as a generic.
-   * @param <C> The number of columns of the desired matrix as a generic.
-   * @return A builder to construct the matrix.
-   */
-  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
-    return new MatBuilder<>(rows, cols);
-  }
-
-  /**
-   * Entrypoint to the VecBuilder class for creation
-   * of custom vectors with the given size and contents.
-   *
-   * @param dim The dimension of the vector.
-   * @param <D> The dimension of the vector as a generic.
-   * @return A builder to construct the vector.
-   */
-  public static <D extends Num> VecBuilder<D> vec(Nat<D> dim) {
-    return new VecBuilder<>(dim);
-  }
-}

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java
deleted file mode 100644
index c7385ea..0000000
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/Num.java
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-/**
- * A number expressed as a java class.
- */
-public abstract class Num {
-  /**
-   * The number this is backing.
-   *
-   * @return The number represented by this class.
-   */
-  public abstract int getNum();
-}

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
deleted file mode 100644
index ea983d4..0000000
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
+++ /dev/null
@@ -1,161 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.function.BiFunction;
-
-import org.ejml.dense.row.NormOps_DDRM;
-import org.ejml.simple.SimpleBase;
-import org.ejml.simple.SimpleMatrix;
-
-public class SimpleMatrixUtils {
-  private SimpleMatrixUtils() {}
-
-  /**
-   * Compute the matrix exponential, e^M of the given matrix.
-   *
-   * @param matrix The matrix to compute the exponential of.
-   * @return The resultant matrix.
-   */
-  @SuppressWarnings({"LocalVariableName", "LineLength"})
-  public static SimpleMatrix expm(SimpleMatrix matrix) {
-    BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
-    SimpleMatrix A = matrix;
-    double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
-    int n_squarings = 0;
-
-    if (A_L1 < 1.495585217958292e-002) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else if (A_L1 < 2.539398330063230e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else if (A_L1 < 9.504178996162932e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else if (A_L1 < 2.097847961257068e+000) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else {
-      double maxNorm = 5.371920351148152;
-      double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
-      n_squarings = (int) Math.max(0, Math.ceil(log));
-      A = A.divide(Math.pow(2.0, n_squarings));
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade13(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    }
-  }
-
-  @SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
-  private static SimpleMatrix dispatchPade(SimpleMatrix U, SimpleMatrix V,
-                                           int nSquarings, BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
-    SimpleMatrix P = U.plus(V);
-    SimpleMatrix Q = U.negative().plus(V);
-
-    SimpleMatrix R = solveProvider.apply(Q, P);
-
-    for (int i = 0; i < nSquarings; i++) {
-      R = R.mult(R);
-    }
-
-    return R;
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
-    double[] b = new double[]{120, 60, 12, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
-    SimpleMatrix V = A2.scale(b[2]).plus(ident.scale(b[0]));
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
-    double[] b = new double[]{30240, 15120, 3360, 420, 30, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-
-    SimpleMatrix U = A.mult(A4.scale(b[5]).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V = A4.scale(b[4]).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
-    double[] b = new double[]{17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-
-    SimpleMatrix U = A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V = A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
-    double[] b = new double[]{17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240,
-        2162160, 110880, 3960, 90, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-    SimpleMatrix A8 = A6.mult(A2);
-
-    SimpleMatrix U = A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V = A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
-    double[] b = new double[]{64764752532480000.0, 32382376266240000.0, 7771770303897600.0,
-        1187353796428800.0, 129060195264000.0, 10559470521600.0, 670442572800.0,
-        33522128640.0, 1323241920, 40840800, 960960, 16380, 182, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-
-    SimpleMatrix U = A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V = A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
-
-    return new Pair<>(U, V);
-  }
-
-  private static SimpleMatrix eye(int rows, int cols) {
-    return SimpleMatrix.identity(Math.min(rows, cols));
-  }
-
-  private static class Pair<A, B> {
-    private final A m_first;
-    private final B m_second;
-
-    Pair(A first, B second) {
-      m_first = first;
-      m_second = second;
-    }
-
-    public A getFirst() {
-      return m_first;
-    }
-
-    public B getSecond() {
-      return m_second;
-    }
-  }
-}

wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java

diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
deleted file mode 100644
index deaaa41..0000000
--- a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
- *
- * @param <N> The dimension of the vector to be constructed.
- */
-public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
-  public VecBuilder(Nat<N> rows) {
-    super(rows, Nat.N1());
-  }
-}

wpiutil/src/main/native/cpp/HttpUtil.cpp

diff --git a/wpiutil/src/main/native/cpp/HttpUtil.cpp b/wpiutil/src/main/native/cpp/HttpUtil.cpp
index 37fea60..866ee10 100644
--- a/wpiutil/src/main/native/cpp/HttpUtil.cpp
+++ b/wpiutil/src/main/native/cpp/HttpUtil.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -86,6 +86,63 @@
   return StringRef{buf.data(), buf.size()};
 }
 
+HttpQueryMap::HttpQueryMap(wpi::StringRef query) {
+  wpi::SmallVector<wpi::StringRef, 16> queryElems;
+  query.split(queryElems, '&', 100, false);
+  for (auto elem : queryElems) {
+    auto [nameEsc, valueEsc] = elem.split('=');
+    wpi::SmallString<64> nameBuf;
+    bool err = false;
+    auto name = wpi::UnescapeURI(nameEsc, nameBuf, &err);
+    // note: ignores duplicates
+    if (!err) m_elems.try_emplace(name, valueEsc);
+  }
+}
+
+std::optional<wpi::StringRef> HttpQueryMap::Get(
+    wpi::StringRef name, wpi::SmallVectorImpl<char>& buf) const {
+  auto it = m_elems.find(name);
+  if (it == m_elems.end()) return {};
+  bool err = false;
+  auto val = wpi::UnescapeURI(it->second, buf, &err);
+  if (err) return {};
+  return val;
+}
+
+HttpPath::HttpPath(wpi::StringRef path) {
+  // special-case root path to be a single empty element
+  if (path == "/") {
+    m_pathEnds.emplace_back(0);
+    return;
+  }
+  wpi::SmallVector<wpi::StringRef, 16> pathElems;
+  path.split(pathElems, '/', 100, false);
+  for (auto elem : pathElems) {
+    wpi::SmallString<64> buf;
+    bool err = false;
+    auto val = wpi::UnescapeURI(elem, buf, &err);
+    if (err) {
+      m_pathEnds.clear();
+      return;
+    }
+    m_pathBuf += val;
+    m_pathEnds.emplace_back(m_pathBuf.size());
+  }
+}
+
+bool HttpPath::startswith(size_t start, ArrayRef<StringRef> match) const {
+  if (m_pathEnds.size() < (start + match.size())) return false;
+  bool first = start == 0;
+  auto p = m_pathEnds.begin() + start;
+  for (auto m : match) {
+    auto val = m_pathBuf.slice(first ? 0 : *(p - 1), *p);
+    if (val != m) return false;
+    first = false;
+    ++p;
+  }
+  return true;
+}
+
 bool ParseHttpHeaders(raw_istream& is, SmallVectorImpl<char>* contentType,
                       SmallVectorImpl<char>* contentLength) {
   if (contentType) contentType->clear();

wpiutil/src/main/native/cpp/MimeTypes.cpp

diff --git a/wpiutil/src/main/native/cpp/MimeTypes.cpp b/wpiutil/src/main/native/cpp/MimeTypes.cpp
new file mode 100644
index 0000000..5bf9d29
--- /dev/null
+++ b/wpiutil/src/main/native/cpp/MimeTypes.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "wpi/MimeTypes.h"
+
+#include "wpi/StringMap.h"
+
+namespace wpi {
+
+// derived partially from
+// https://github.com/DEGoodmanWilson/libmime/blob/stable/0.1.2/mime/mime.cpp
+StringRef MimeTypeFromPath(StringRef path) {
+  // https://developer.mozilla.org/en-US/docs/Web/HTTP/Basics_of_HTTP/MIME_types
+  static StringMap<const char*> mimeTypes{
+      // text
+      {"css", "text/css"},
+      {"csv", "text/csv"},
+      {"htm", "text/html"},
+      {"html", "text/html"},
+      {"js", "text/javascript"},
+      {"json", "application/json"},
+      {"map", "application/json"},
+      {"md", "text/markdown"},
+      {"txt", "text/plain"},
+      {"xml", "text/xml"},
+
+      // images
+      {"apng", "image/apng"},
+      {"bmp", "image/bmp"},
+      {"gif", "image/gif"},
+      {"cur", "image/x-icon"},
+      {"ico", "image/x-icon"},
+      {"jpg", "image/jpeg"},
+      {"jpeg", "image/jpeg"},
+      {"png", "image/png"},
+      {"svg", "image/svg+xml"},
+      {"tif", "image/tiff"},
+      {"tiff", "image/tiff"},
+      {"webp", "image/webp"},
+
+      // fonts
+      {"otf", "font/otf"},
+      {"ttf", "font/ttf"},
+      {"woff", "font/woff"},
+
+      // misc
+      {"pdf", "application/pdf"},
+      {"zip", "application/zip"},
+  };
+
+  static const char* defaultType = "application/octet-stream";
+
+  auto pos = path.find_last_of("/");
+  if (pos != StringRef::npos) {
+    path = path.substr(pos + 1);
+  }
+  auto dot_pos = path.find_last_of(".");
+  if (dot_pos > 0 && dot_pos != StringRef::npos) {
+    auto type = mimeTypes.find(path.substr(dot_pos + 1));
+    if (type != mimeTypes.end()) {
+      return type->getValue();
+    }
+  }
+  return defaultType;
+}
+
+}  // namespace wpi

wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp

diff --git a/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp b/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp
index de8bc2c..ee5803f 100644
--- a/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp
+++ b/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -107,8 +107,7 @@
       }
       ++result->count;
       result->cv.notify_all();
-    })
-        .detach();
+    }).detach();
   }
 
   // wait for a result, timeout, or all finished

wpiutil/src/main/native/cpp/TCPStream.cpp

diff --git a/wpiutil/src/main/native/cpp/TCPStream.cpp b/wpiutil/src/main/native/cpp/TCPStream.cpp
index d4e3671..4af3e6f 100644
--- a/wpiutil/src/main/native/cpp/TCPStream.cpp
+++ b/wpiutil/src/main/native/cpp/TCPStream.cpp
@@ -1,7 +1,7 @@
 /*
    TCPStream.h
 
-   TCPStream class definition. TCPStream provides methods to trasnfer
+   TCPStream class definition. TCPStream provides methods to transfer
    data between peers over a TCP/IP connection.
 
    ------------------------------------------

wpiutil/src/main/native/cpp/http_parser.cpp

diff --git a/wpiutil/src/main/native/cpp/http_parser.cpp b/wpiutil/src/main/native/cpp/http_parser.cpp
index fb56010..bc442b2 100644
--- a/wpiutil/src/main/native/cpp/http_parser.cpp
+++ b/wpiutil/src/main/native/cpp/http_parser.cpp
@@ -1771,7 +1771,7 @@
         /* Here we call the headers_complete callback. This is somewhat
          * different than other callbacks because if the user returns 1, we
          * will interpret that as saying that this message has no body. This
-         * is needed for the annoying case of recieving a response to a HEAD
+         * is needed for the annoying case of receiving a response to a HEAD
          * request.
          *
          * We'd like to use CALLBACK_NOTIFY_NOADVANCE() here but we cannot, so

wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp

diff --git a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
index c14b3f4..90a2291 100644
--- a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
+++ b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,7 @@
 #include "edu_wpi_first_wpiutil_WPIUtilJNI.h"
 #include "wpi/PortForwarder.h"
 #include "wpi/jni_util.h"
+#include "wpi/timestamp.h"
 
 using namespace wpi::java;
 
@@ -27,6 +28,18 @@
 
 /*
  * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
+ * Method:    now
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_wpiutil_WPIUtilJNI_now
+  (JNIEnv*, jclass)
+{
+  return wpi::Now();
+}
+
+/*
+ * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
  * Method:    addPortForwarder
  * Signature: (ILjava/lang/String;I)V
  */

wpiutil/src/main/native/cpp/json_serializer.h

diff --git a/wpiutil/src/main/native/cpp/json_serializer.h b/wpiutil/src/main/native/cpp/json_serializer.h
index 6983920..6c5ffbe 100644
--- a/wpiutil/src/main/native/cpp/json_serializer.h
+++ b/wpiutil/src/main/native/cpp/json_serializer.h
@@ -36,6 +36,7 @@
 #include <clocale> // lconv, localeconv
 #include <cmath>  // labs, isfinite, isnan, signbit, ldexp
 #include <locale> // locale
+#include <type_traits>
 
 #include "wpi/raw_ostream.h"
 
@@ -99,6 +100,18 @@
     */
     void dump_escaped(StringRef s, const bool ensure_ascii);
 
+    template <typename NumberType,
+              detail::enable_if_t<std::is_same_v<NumberType, uint64_t>, int> = 0>
+    bool is_negative_integer(NumberType x) {
+      return false;
+    }
+
+    template <typename NumberType,
+              detail::enable_if_t<std::is_same_v<NumberType, int64_t>, int> = 0>
+    bool is_negative_integer(NumberType x) {
+      return x < 0;
+    }
+
     /*!
     @brief dump an integer
 
@@ -121,7 +134,7 @@
             return;
         }
 
-        const bool is_negative = (x <= 0) and (x != 0);  // see issue #755
+        const bool is_negative = is_negative_integer(x);  // see issue #755
         std::size_t i = 0;
 
         while (x != 0)

wpiutil/src/main/native/cpp/llvm/Unix/Path.inc

diff --git a/wpiutil/src/main/native/cpp/llvm/Unix/Path.inc b/wpiutil/src/main/native/cpp/llvm/Unix/Path.inc
index b786813..4ec9e86 100644
--- a/wpiutil/src/main/native/cpp/llvm/Unix/Path.inc
+++ b/wpiutil/src/main/native/cpp/llvm/Unix/Path.inc
@@ -410,7 +410,7 @@
     return std::error_code();
   RealPath->clear();
 #if defined(F_GETPATH)
-  // When F_GETPATH is availble, it is the quickest way to get
+  // When F_GETPATH is available, it is the quickest way to get
   // the real path name.
   char Buffer[MAXPATHLEN];
   if (::fcntl(ResultFD, F_GETPATH, Buffer) != -1)

wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp

diff --git a/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp b/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp
index 994aadc..6a4446f 100644
--- a/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp
+++ b/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -19,17 +19,18 @@
 
 void GetNameInfo(Loop& loop, const std::shared_ptr<GetNameInfoReq>& req,
                  const sockaddr& addr, int flags) {
-  int err = uv_getnameinfo(loop.GetRaw(), req->GetRaw(),
-                           [](uv_getnameinfo_t* req, int status,
-                              const char* hostname, const char* service) {
-                             auto& h = *static_cast<GetNameInfoReq*>(req->data);
-                             if (status < 0)
-                               h.ReportError(status);
-                             else
-                               h.resolved(hostname, service);
-                             h.Release();  // this is always a one-shot
-                           },
-                           &addr, flags);
+  int err = uv_getnameinfo(
+      loop.GetRaw(), req->GetRaw(),
+      [](uv_getnameinfo_t* req, int status, const char* hostname,
+         const char* service) {
+        auto& h = *static_cast<GetNameInfoReq*>(req->data);
+        if (status < 0)
+          h.ReportError(status);
+        else
+          h.resolved(hostname, service);
+        h.Release();  // this is always a one-shot
+      },
+      &addr, flags);
   if (err < 0)
     loop.ReportError(err);
   else

wpiutil/src/main/native/cpp/uv/Loop.cpp

diff --git a/wpiutil/src/main/native/cpp/uv/Loop.cpp b/wpiutil/src/main/native/cpp/uv/Loop.cpp
index 2602150..a31ad09 100644
--- a/wpiutil/src/main/native/cpp/uv/Loop.cpp
+++ b/wpiutil/src/main/native/cpp/uv/Loop.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -49,13 +49,14 @@
 }
 
 void Loop::Walk(std::function<void(Handle&)> callback) {
-  uv_walk(m_loop,
-          [](uv_handle_t* handle, void* func) {
-            auto& h = *static_cast<Handle*>(handle->data);
-            auto& f = *static_cast<std::function<void(Handle&)>*>(func);
-            f(h);
-          },
-          &callback);
+  uv_walk(
+      m_loop,
+      [](uv_handle_t* handle, void* func) {
+        auto& h = *static_cast<Handle*>(handle->data);
+        auto& f = *static_cast<std::function<void(Handle&)>*>(func);
+        f(h);
+      },
+      &callback);
 }
 
 void Loop::Fork() {

wpiutil/src/main/native/cpp/uv/Signal.cpp

diff --git a/wpiutil/src/main/native/cpp/uv/Signal.cpp b/wpiutil/src/main/native/cpp/uv/Signal.cpp
index 083b852..4569ac2 100644
--- a/wpiutil/src/main/native/cpp/uv/Signal.cpp
+++ b/wpiutil/src/main/native/cpp/uv/Signal.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -24,12 +24,13 @@
 }
 
 void Signal::Start(int signum) {
-  Invoke(&uv_signal_start, GetRaw(),
-         [](uv_signal_t* handle, int signum) {
-           Signal& h = *static_cast<Signal*>(handle->data);
-           h.signal(signum);
-         },
-         signum);
+  Invoke(
+      &uv_signal_start, GetRaw(),
+      [](uv_signal_t* handle, int signum) {
+        Signal& h = *static_cast<Signal*>(handle->data);
+        h.signal(signum);
+      },
+      signum);
 }
 
 }  // namespace uv

wpiutil/src/main/native/cpp/uv/Timer.cpp

diff --git a/wpiutil/src/main/native/cpp/uv/Timer.cpp b/wpiutil/src/main/native/cpp/uv/Timer.cpp
index 749d9a8..0fdd3be 100644
--- a/wpiutil/src/main/native/cpp/uv/Timer.cpp
+++ b/wpiutil/src/main/native/cpp/uv/Timer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -34,12 +34,13 @@
 }
 
 void Timer::Start(Time timeout, Time repeat) {
-  Invoke(&uv_timer_start, GetRaw(),
-         [](uv_timer_t* handle) {
-           Timer& h = *static_cast<Timer*>(handle->data);
-           h.timeout();
-         },
-         timeout.count(), repeat.count());
+  Invoke(
+      &uv_timer_start, GetRaw(),
+      [](uv_timer_t* handle) {
+        Timer& h = *static_cast<Timer*>(handle->data);
+        h.timeout();
+      },
+      timeout.count(), repeat.count());
 }
 
 }  // namespace uv

wpiutil/src/main/native/cpp/uv/Work.cpp

diff --git a/wpiutil/src/main/native/cpp/uv/Work.cpp b/wpiutil/src/main/native/cpp/uv/Work.cpp
index d71ef81..e5d7bb6 100644
--- a/wpiutil/src/main/native/cpp/uv/Work.cpp
+++ b/wpiutil/src/main/native/cpp/uv/Work.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -17,19 +17,20 @@
 }
 
 void QueueWork(Loop& loop, const std::shared_ptr<WorkReq>& req) {
-  int err = uv_queue_work(loop.GetRaw(), req->GetRaw(),
-                          [](uv_work_t* req) {
-                            auto& h = *static_cast<WorkReq*>(req->data);
-                            h.work();
-                          },
-                          [](uv_work_t* req, int status) {
-                            auto& h = *static_cast<WorkReq*>(req->data);
-                            if (status < 0)
-                              h.ReportError(status);
-                            else
-                              h.afterWork();
-                            h.Release();  // this is always a one-shot
-                          });
+  int err = uv_queue_work(
+      loop.GetRaw(), req->GetRaw(),
+      [](uv_work_t* req) {
+        auto& h = *static_cast<WorkReq*>(req->data);
+        h.work();
+      },
+      [](uv_work_t* req, int status) {
+        auto& h = *static_cast<WorkReq*>(req->data);
+        if (status < 0)
+          h.ReportError(status);
+        else
+          h.afterWork();
+        h.Release();  // this is always a one-shot
+      });
   if (err < 0)
     loop.ReportError(err);
   else

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
deleted file mode 100644
index 3dbc708..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
+++ /dev/null
@@ -1,226 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_ARRAYBASE_H
-#define EIGEN_ARRAYBASE_H
-
-namespace Eigen { 
-
-template<typename ExpressionType> class MatrixWrapper;
-
-/** \class ArrayBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for all 1D and 2D array, and related expressions
-  *
-  * An array is similar to a dense vector or matrix. While matrices are mathematical
-  * objects with well defined linear algebra operators, an array is just a collection
-  * of scalar values arranged in a one or two dimensionnal fashion. As the main consequence,
-  * all operations applied to an array are performed coefficient wise. Furthermore,
-  * arrays support scalar math functions of the c++ standard library (e.g., std::sin(x)), and convenient
-  * constructors allowing to easily write generic code working for both scalar values
-  * and arrays.
-  *
-  * This class is the base that is inherited by all array expression types.
-  *
-  * \tparam Derived is the derived type, e.g., an array or an expression type.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_ARRAYBASE_PLUGIN.
-  *
-  * \sa class MatrixBase, \ref TopicClassHierarchy
-  */
-template<typename Derived> class ArrayBase
-  : public DenseBase<Derived>
-{
-  public:
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** The base class for a given storage type. */
-    typedef ArrayBase StorageBaseType;
-
-    typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl;
-
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-
-    typedef DenseBase<Derived> Base;
-    using Base::RowsAtCompileTime;
-    using Base::ColsAtCompileTime;
-    using Base::SizeAtCompileTime;
-    using Base::MaxRowsAtCompileTime;
-    using Base::MaxColsAtCompileTime;
-    using Base::MaxSizeAtCompileTime;
-    using Base::IsVectorAtCompileTime;
-    using Base::Flags;
-    
-    using Base::derived;
-    using Base::const_cast_derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
-    using Base::lazyAssign;
-    using Base::operator=;
-    using Base::operator+=;
-    using Base::operator-=;
-    using Base::operator*=;
-    using Base::operator/=;
-
-    typedef typename Base::CoeffReturnType CoeffReturnType;
-
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef typename Base::PlainObject PlainObject;
-
-    /** \internal Represents a matrix with all coefficients equal to one another*/
-    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::ArrayBase
-#define EIGEN_DOC_UNARY_ADDONS(X,Y)
-#   include "../plugins/CommonCwiseUnaryOps.h"
-#   include "../plugins/MatrixCwiseUnaryOps.h"
-#   include "../plugins/ArrayCwiseUnaryOps.h"
-#   include "../plugins/CommonCwiseBinaryOps.h"
-#   include "../plugins/MatrixCwiseBinaryOps.h"
-#   include "../plugins/ArrayCwiseBinaryOps.h"
-#   ifdef EIGEN_ARRAYBASE_PLUGIN
-#     include EIGEN_ARRAYBASE_PLUGIN
-#   endif
-#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
-#undef EIGEN_DOC_UNARY_ADDONS
-
-    /** Special case of the template operator=, in order to prevent the compiler
-      * from generating a default operator= (issue hit with g++ 4.1)
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const ArrayBase& other)
-    {
-      internal::call_assignment(derived(), other.derived());
-      return derived();
-    }
-    
-    /** Set all the entries to \a value.
-      * \sa DenseBase::setConstant(), DenseBase::fill() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const Scalar &value)
-    { Base::setConstant(value); return derived(); }
-
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator+=(const Scalar& scalar);
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator-=(const Scalar& scalar);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator+=(const ArrayBase<OtherDerived>& other);
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator-=(const ArrayBase<OtherDerived>& other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator*=(const ArrayBase<OtherDerived>& other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator/=(const ArrayBase<OtherDerived>& other);
-
-  public:
-    EIGEN_DEVICE_FUNC
-    ArrayBase<Derived>& array() { return *this; }
-    EIGEN_DEVICE_FUNC
-    const ArrayBase<Derived>& array() const { return *this; }
-
-    /** \returns an \link Eigen::MatrixBase Matrix \endlink expression of this array
-      * \sa MatrixBase::array() */
-    EIGEN_DEVICE_FUNC
-    MatrixWrapper<Derived> matrix() { return MatrixWrapper<Derived>(derived()); }
-    EIGEN_DEVICE_FUNC
-    const MatrixWrapper<const Derived> matrix() const { return MatrixWrapper<const Derived>(derived()); }
-
-//     template<typename Dest>
-//     inline void evalTo(Dest& dst) const { dst = matrix(); }
-
-  protected:
-    EIGEN_DEVICE_FUNC
-    ArrayBase() : Base() {}
-
-  private:
-    explicit ArrayBase(Index);
-    ArrayBase(Index,Index);
-    template<typename OtherDerived> explicit ArrayBase(const ArrayBase<OtherDerived>&);
-  protected:
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator+=(const MatrixBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator-=(const MatrixBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
-};
-
-/** replaces \c *this by \c *this - \a other.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator-=(const ArrayBase<OtherDerived> &other)
-{
-  call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
-  return derived();
-}
-
-/** replaces \c *this by \c *this + \a other.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator+=(const ArrayBase<OtherDerived>& other)
-{
-  call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
-  return derived();
-}
-
-/** replaces \c *this by \c *this * \a other coefficient wise.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator*=(const ArrayBase<OtherDerived>& other)
-{
-  call_assignment(derived(), other.derived(), internal::mul_assign_op<Scalar,typename OtherDerived::Scalar>());
-  return derived();
-}
-
-/** replaces \c *this by \c *this / \a other coefficient wise.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
-ArrayBase<Derived>::operator/=(const ArrayBase<OtherDerived>& other)
-{
-  call_assignment(derived(), other.derived(), internal::div_assign_op<Scalar,typename OtherDerived::Scalar>());
-  return derived();
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_ARRAYBASE_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
deleted file mode 100644
index 2710330..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
+++ /dev/null
@@ -1,128 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_CWISE_UNARY_VIEW_H
-#define EIGEN_CWISE_UNARY_VIEW_H
-
-namespace Eigen {
-
-namespace internal {
-template<typename ViewOp, typename MatrixType>
-struct traits<CwiseUnaryView<ViewOp, MatrixType> >
- : traits<MatrixType>
-{
-  typedef typename result_of<
-                     ViewOp(const typename traits<MatrixType>::Scalar&)
-                   >::type Scalar;
-  typedef typename MatrixType::Nested MatrixTypeNested;
-  typedef typename remove_all<MatrixTypeNested>::type _MatrixTypeNested;
-  enum {
-    FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
-    Flags = traits<_MatrixTypeNested>::Flags & (RowMajorBit | FlagsLvalueBit | DirectAccessBit), // FIXME DirectAccessBit should not be handled by expressions
-    MatrixTypeInnerStride =  inner_stride_at_compile_time<MatrixType>::ret,
-    // need to cast the sizeof's from size_t to int explicitly, otherwise:
-    // "error: no integral type can represent all of the enumerator values
-    InnerStrideAtCompileTime = MatrixTypeInnerStride == Dynamic
-                             ? int(Dynamic)
-                             : int(MatrixTypeInnerStride) * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar)),
-    OuterStrideAtCompileTime = outer_stride_at_compile_time<MatrixType>::ret == Dynamic
-                             ? int(Dynamic)
-                             : outer_stride_at_compile_time<MatrixType>::ret * int(sizeof(typename traits<MatrixType>::Scalar) / sizeof(Scalar))
-  };
-};
-}
-
-template<typename ViewOp, typename MatrixType, typename StorageKind>
-class CwiseUnaryViewImpl;
-
-/** \class CwiseUnaryView
-  * \ingroup Core_Module
-  *
-  * \brief Generic lvalue expression of a coefficient-wise unary operator of a matrix or a vector
-  *
-  * \tparam ViewOp template functor implementing the view
-  * \tparam MatrixType the type of the matrix we are applying the unary operator
-  *
-  * This class represents a lvalue expression of a generic unary view operator of a matrix or a vector.
-  * It is the return type of real() and imag(), and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::unaryViewExpr(const CustomUnaryOp &) const, class CwiseUnaryOp
-  */
-template<typename ViewOp, typename MatrixType>
-class CwiseUnaryView : public CwiseUnaryViewImpl<ViewOp, MatrixType, typename internal::traits<MatrixType>::StorageKind>
-{
-  public:
-
-    typedef typename CwiseUnaryViewImpl<ViewOp, MatrixType,typename internal::traits<MatrixType>::StorageKind>::Base Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryView)
-    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
-    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
-
-    explicit inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp())
-      : m_matrix(mat), m_functor(func) {}
-
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView)
-
-    EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); }
-    EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); }
-
-    /** \returns the functor representing unary operation */
-    const ViewOp& functor() const { return m_functor; }
-
-    /** \returns the nested expression */
-    const typename internal::remove_all<MatrixTypeNested>::type&
-    nestedExpression() const { return m_matrix; }
-
-    /** \returns the nested expression */
-    typename internal::remove_reference<MatrixTypeNested>::type&
-    nestedExpression() { return m_matrix.const_cast_derived(); }
-
-  protected:
-    MatrixTypeNested m_matrix;
-    ViewOp m_functor;
-};
-
-// Generic API dispatcher
-template<typename ViewOp, typename XprType, typename StorageKind>
-class CwiseUnaryViewImpl
-  : public internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type
-{
-public:
-  typedef typename internal::generic_xpr_base<CwiseUnaryView<ViewOp, XprType> >::type Base;
-};
-
-template<typename ViewOp, typename MatrixType>
-class CwiseUnaryViewImpl<ViewOp,MatrixType,Dense>
-  : public internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type
-{
-  public:
-
-    typedef CwiseUnaryView<ViewOp, MatrixType> Derived;
-    typedef typename internal::dense_xpr_base< CwiseUnaryView<ViewOp, MatrixType> >::type Base;
-
-    EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl)
-    
-    EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); }
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); }
-
-    EIGEN_DEVICE_FUNC inline Index innerStride() const
-    {
-      return derived().nestedExpression().innerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
-    }
-
-    EIGEN_DEVICE_FUNC inline Index outerStride() const
-    {
-      return derived().nestedExpression().outerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
-    }
-};
-
-} // end namespace Eigen
-
-#endif // EIGEN_CWISE_UNARY_VIEW_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
deleted file mode 100644
index 90066ae..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
+++ /dev/null
@@ -1,611 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2007-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
-// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_DENSEBASE_H
-#define EIGEN_DENSEBASE_H
-
-namespace Eigen {
-
-namespace internal {
-  
-// The index type defined by EIGEN_DEFAULT_DENSE_INDEX_TYPE must be a signed type.
-// This dummy function simply aims at checking that at compile time.
-static inline void check_DenseIndex_is_signed() {
-  EIGEN_STATIC_ASSERT(NumTraits<DenseIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE); 
-}
-
-} // end namespace internal
-  
-/** \class DenseBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for all dense matrices, vectors, and arrays
-  *
-  * This class is the base that is inherited by all dense objects (matrix, vector, arrays,
-  * and related expression types). The common Eigen API for dense objects is contained in this class.
-  *
-  * \tparam Derived is the derived type, e.g., a matrix type or an expression.
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_DENSEBASE_PLUGIN.
-  *
-  * \sa \blank \ref TopicClassHierarchy
-  */
-template<typename Derived> class DenseBase
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-  : public DenseCoeffsBase<Derived>
-#else
-  : public DenseCoeffsBase<Derived,DirectWriteAccessors>
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-{
-  public:
-
-    /** Inner iterator type to iterate over the coefficients of a row or column.
-      * \sa class InnerIterator
-      */
-    typedef Eigen::InnerIterator<Derived> InnerIterator;
-
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-
-    /**
-      * \brief The type used to store indices
-      * \details This typedef is relevant for types that store multiple indices such as
-      *          PermutationMatrix or Transpositions, otherwise it defaults to Eigen::Index
-      * \sa \blank \ref TopicPreprocessorDirectives, Eigen::Index, SparseMatrixBase.
-     */
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
-
-    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc. */
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    
-    /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
-      *
-      * It is an alias for the Scalar type */
-    typedef Scalar value_type;
-    
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef DenseCoeffsBase<Derived> Base;
-
-    using Base::derived;
-    using Base::const_cast_derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::rowIndexByOuterInner;
-    using Base::colIndexByOuterInner;
-    using Base::coeff;
-    using Base::coeffByOuterInner;
-    using Base::operator();
-    using Base::operator[];
-    using Base::x;
-    using Base::y;
-    using Base::z;
-    using Base::w;
-    using Base::stride;
-    using Base::innerStride;
-    using Base::outerStride;
-    using Base::rowStride;
-    using Base::colStride;
-    typedef typename Base::CoeffReturnType CoeffReturnType;
-
-    enum {
-
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-        /**< The number of rows at compile-time. This is just a copy of the value provided
-          * by the \a Derived type. If a value is not known at compile-time,
-          * it is set to the \a Dynamic constant.
-          * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
-
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-        /**< The number of columns at compile-time. This is just a copy of the value provided
-          * by the \a Derived type. If a value is not known at compile-time,
-          * it is set to the \a Dynamic constant.
-          * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
-
-
-      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
-                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
-        /**< This is equal to the number of coefficients, i.e. the number of
-          * rows times the number of columns, or to \a Dynamic if this is not
-          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
-
-      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
-        /**< This value is equal to the maximum possible number of rows that this expression
-          * might have. If this expression might have an arbitrarily high number of rows,
-          * this value is set to \a Dynamic.
-          *
-          * This value is useful to know when evaluating an expression, in order to determine
-          * whether it is possible to avoid doing a dynamic memory allocation.
-          *
-          * \sa RowsAtCompileTime, MaxColsAtCompileTime, MaxSizeAtCompileTime
-          */
-
-      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
-        /**< This value is equal to the maximum possible number of columns that this expression
-          * might have. If this expression might have an arbitrarily high number of columns,
-          * this value is set to \a Dynamic.
-          *
-          * This value is useful to know when evaluating an expression, in order to determine
-          * whether it is possible to avoid doing a dynamic memory allocation.
-          *
-          * \sa ColsAtCompileTime, MaxRowsAtCompileTime, MaxSizeAtCompileTime
-          */
-
-      MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
-                                                      internal::traits<Derived>::MaxColsAtCompileTime>::ret),
-        /**< This value is equal to the maximum possible number of coefficients that this expression
-          * might have. If this expression might have an arbitrarily high number of coefficients,
-          * this value is set to \a Dynamic.
-          *
-          * This value is useful to know when evaluating an expression, in order to determine
-          * whether it is possible to avoid doing a dynamic memory allocation.
-          *
-          * \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime
-          */
-
-      IsVectorAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime == 1
-                           || internal::traits<Derived>::MaxColsAtCompileTime == 1,
-        /**< This is set to true if either the number of rows or the number of
-          * columns is known at compile-time to be equal to 1. Indeed, in that case,
-          * we are dealing with a column-vector (if there is only one column) or with
-          * a row-vector (if there is only one row). */
-
-      Flags = internal::traits<Derived>::Flags,
-        /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
-          * constructed from this one. See the \ref flags "list of flags".
-          */
-
-      IsRowMajor = int(Flags) & RowMajorBit, /**< True if this expression has row-major storage order. */
-
-      InnerSizeAtCompileTime = int(IsVectorAtCompileTime) ? int(SizeAtCompileTime)
-                             : int(IsRowMajor) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
-
-      InnerStrideAtCompileTime = internal::inner_stride_at_compile_time<Derived>::ret,
-      OuterStrideAtCompileTime = internal::outer_stride_at_compile_time<Derived>::ret
-    };
-    
-    typedef typename internal::find_best_packet<Scalar,SizeAtCompileTime>::type PacketScalar;
-
-    enum { IsPlainObjectBase = 0 };
-    
-    /** The plain matrix type corresponding to this expression.
-      * \sa PlainObject */
-    typedef Matrix<typename internal::traits<Derived>::Scalar,
-                internal::traits<Derived>::RowsAtCompileTime,
-                internal::traits<Derived>::ColsAtCompileTime,
-                AutoAlign | (internal::traits<Derived>::Flags&RowMajorBit ? RowMajor : ColMajor),
-                internal::traits<Derived>::MaxRowsAtCompileTime,
-                internal::traits<Derived>::MaxColsAtCompileTime
-          > PlainMatrix;
-    
-    /** The plain array type corresponding to this expression.
-      * \sa PlainObject */
-    typedef Array<typename internal::traits<Derived>::Scalar,
-                internal::traits<Derived>::RowsAtCompileTime,
-                internal::traits<Derived>::ColsAtCompileTime,
-                AutoAlign | (internal::traits<Derived>::Flags&RowMajorBit ? RowMajor : ColMajor),
-                internal::traits<Derived>::MaxRowsAtCompileTime,
-                internal::traits<Derived>::MaxColsAtCompileTime
-          > PlainArray;
-
-    /** \brief The plain matrix or array type corresponding to this expression.
-      *
-      * This is not necessarily exactly the return type of eval(). In the case of plain matrices,
-      * the return type of eval() is a const reference to a matrix, not a matrix! It is however guaranteed
-      * that the return type of eval() is either PlainObject or const PlainObject&.
-      */
-    typedef typename internal::conditional<internal::is_same<typename internal::traits<Derived>::XprKind,MatrixXpr >::value,
-                                 PlainMatrix, PlainArray>::type PlainObject;
-
-    /** \returns the number of nonzero coefficients which is in practice the number
-      * of stored coefficients. */
-    EIGEN_DEVICE_FUNC
-    inline Index nonZeros() const { return size(); }
-
-    /** \returns the outer size.
-      *
-      * \note For a vector, this returns just 1. For a matrix (non-vector), this is the major dimension
-      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of columns for a
-      * column-major matrix, and the number of rows for a row-major matrix. */
-    EIGEN_DEVICE_FUNC
-    Index outerSize() const
-    {
-      return IsVectorAtCompileTime ? 1
-           : int(IsRowMajor) ? this->rows() : this->cols();
-    }
-
-    /** \returns the inner size.
-      *
-      * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension
-      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a 
-      * column-major matrix, and the number of columns for a row-major matrix. */
-    EIGEN_DEVICE_FUNC
-    Index innerSize() const
-    {
-      return IsVectorAtCompileTime ? this->size()
-           : int(IsRowMajor) ? this->cols() : this->rows();
-    }
-
-    /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
-      * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does
-      * nothing else.
-      */
-    EIGEN_DEVICE_FUNC
-    void resize(Index newSize)
-    {
-      EIGEN_ONLY_USED_FOR_DEBUG(newSize);
-      eigen_assert(newSize == this->size()
-                && "DenseBase::resize() does not actually allow to resize.");
-    }
-    /** Only plain matrices/arrays, not expressions, may be resized; therefore the only useful resize methods are
-      * Matrix::resize() and Array::resize(). The present method only asserts that the new size equals the old size, and does
-      * nothing else.
-      */
-    EIGEN_DEVICE_FUNC
-    void resize(Index rows, Index cols)
-    {
-      EIGEN_ONLY_USED_FOR_DEBUG(rows);
-      EIGEN_ONLY_USED_FOR_DEBUG(cols);
-      eigen_assert(rows == this->rows() && cols == this->cols()
-                && "DenseBase::resize() does not actually allow to resize.");
-    }
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal Represents a matrix with all coefficients equal to one another*/
-    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
-    /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */
-    typedef CwiseNullaryOp<internal::linspaced_op<Scalar,PacketScalar>,PlainObject> SequentialLinSpacedReturnType;
-    /** \internal Represents a vector with linearly spaced coefficients that allows random access. */
-    typedef CwiseNullaryOp<internal::linspaced_op<Scalar,PacketScalar>,PlainObject> RandomAccessLinSpacedReturnType;
-    /** \internal the return type of MatrixBase::eigenvalues() */
-    typedef Matrix<typename NumTraits<typename internal::traits<Derived>::Scalar>::Real, internal::traits<Derived>::ColsAtCompileTime, 1> EigenvaluesReturnType;
-
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-    /** Copies \a other into *this. \returns a reference to *this. */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const DenseBase<OtherDerived>& other);
-
-    /** Special case of the template operator=, in order to prevent the compiler
-      * from generating a default operator= (issue hit with g++ 4.1)
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const DenseBase& other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const EigenBase<OtherDerived> &other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator+=(const EigenBase<OtherDerived> &other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator-=(const EigenBase<OtherDerived> &other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const ReturnByValue<OtherDerived>& func);
-
-    /** \internal
-      * Copies \a other into *this without evaluating other. \returns a reference to *this.
-      * \deprecated */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& lazyAssign(const DenseBase<OtherDerived>& other);
-
-    EIGEN_DEVICE_FUNC
-    CommaInitializer<Derived> operator<< (const Scalar& s);
-
-    /** \deprecated it now returns \c *this */
-    template<unsigned int Added,unsigned int Removed>
-    EIGEN_DEPRECATED
-    const Derived& flagged() const
-    { return derived(); }
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    CommaInitializer<Derived> operator<< (const DenseBase<OtherDerived>& other);
-
-    typedef Transpose<Derived> TransposeReturnType;
-    EIGEN_DEVICE_FUNC
-    TransposeReturnType transpose();
-    typedef typename internal::add_const<Transpose<const Derived> >::type ConstTransposeReturnType;
-    EIGEN_DEVICE_FUNC
-    ConstTransposeReturnType transpose() const;
-    EIGEN_DEVICE_FUNC
-    void transposeInPlace();
-
-    EIGEN_DEVICE_FUNC static const ConstantReturnType
-    Constant(Index rows, Index cols, const Scalar& value);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType
-    Constant(Index size, const Scalar& value);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType
-    Constant(const Scalar& value);
-
-    EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType
-    LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
-    LinSpaced(Index size, const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType
-    LinSpaced(Sequential_t, const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
-    LinSpaced(const Scalar& low, const Scalar& high);
-
-    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
-    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
-    NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func);
-    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
-    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
-    NullaryExpr(Index size, const CustomNullaryOp& func);
-    template<typename CustomNullaryOp> EIGEN_DEVICE_FUNC
-    static const CwiseNullaryOp<CustomNullaryOp, PlainObject>
-    NullaryExpr(const CustomNullaryOp& func);
-
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero(Index size);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Zero();
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones(Index size);
-    EIGEN_DEVICE_FUNC static const ConstantReturnType Ones();
-
-    EIGEN_DEVICE_FUNC void fill(const Scalar& value);
-    EIGEN_DEVICE_FUNC Derived& setConstant(const Scalar& value);
-    EIGEN_DEVICE_FUNC Derived& setLinSpaced(Index size, const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC Derived& setLinSpaced(const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC Derived& setZero();
-    EIGEN_DEVICE_FUNC Derived& setOnes();
-    EIGEN_DEVICE_FUNC Derived& setRandom();
-
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC
-    bool isApprox(const DenseBase<OtherDerived>& other,
-                  const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC 
-    bool isMuchSmallerThan(const RealScalar& other,
-                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC
-    bool isMuchSmallerThan(const DenseBase<OtherDerived>& other,
-                           const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-
-    EIGEN_DEVICE_FUNC bool isApproxToConstant(const Scalar& value, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC bool isConstant(const Scalar& value, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC bool isZero(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC bool isOnes(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    
-    inline bool hasNaN() const;
-    inline bool allFinite() const;
-
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator*=(const Scalar& other);
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator/=(const Scalar& other);
-
-    typedef typename internal::add_const_on_value_type<typename internal::eval<Derived>::type>::type EvalReturnType;
-    /** \returns the matrix or vector obtained by evaluating this expression.
-      *
-      * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
-      * a const reference, in order to avoid a useless copy.
-      * 
-      * \warning Be carefull with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page \endlink.
-      */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE EvalReturnType eval() const
-    {
-      // Even though MSVC does not honor strong inlining when the return type
-      // is a dynamic matrix, we desperately need strong inlining for fixed
-      // size types on MSVC.
-      return typename internal::eval<Derived>::type(derived());
-    }
-    
-    /** swaps *this with the expression \a other.
-      *
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void swap(const DenseBase<OtherDerived>& other)
-    {
-      EIGEN_STATIC_ASSERT(!OtherDerived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
-      eigen_assert(rows()==other.rows() && cols()==other.cols());
-      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
-    }
-
-    /** swaps *this with the matrix or array \a other.
-      *
-      */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void swap(PlainObjectBase<OtherDerived>& other)
-    {
-      eigen_assert(rows()==other.rows() && cols()==other.cols());
-      call_assignment(derived(), other.derived(), internal::swap_assign_op<Scalar>());
-    }
-
-    EIGEN_DEVICE_FUNC inline const NestByValue<Derived> nestByValue() const;
-    EIGEN_DEVICE_FUNC inline const ForceAlignedAccess<Derived> forceAlignedAccess() const;
-    EIGEN_DEVICE_FUNC inline ForceAlignedAccess<Derived> forceAlignedAccess();
-    template<bool Enable> EIGEN_DEVICE_FUNC
-    inline const typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type forceAlignedAccessIf() const;
-    template<bool Enable> EIGEN_DEVICE_FUNC
-    inline typename internal::conditional<Enable,ForceAlignedAccess<Derived>,Derived&>::type forceAlignedAccessIf();
-
-    EIGEN_DEVICE_FUNC Scalar sum() const;
-    EIGEN_DEVICE_FUNC Scalar mean() const;
-    EIGEN_DEVICE_FUNC Scalar trace() const;
-
-    EIGEN_DEVICE_FUNC Scalar prod() const;
-
-    EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar minCoeff() const;
-    EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar maxCoeff() const;
-
-    template<typename IndexType> EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const;
-    template<typename IndexType> EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const;
-    template<typename IndexType> EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const;
-    template<typename IndexType> EIGEN_DEVICE_FUNC
-    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const;
-
-    template<typename BinaryOp>
-    EIGEN_DEVICE_FUNC
-    Scalar redux(const BinaryOp& func) const;
-
-    template<typename Visitor>
-    EIGEN_DEVICE_FUNC
-    void visit(Visitor& func) const;
-
-    /** \returns a WithFormat proxy object allowing to print a matrix the with given
-      * format \a fmt.
-      *
-      * See class IOFormat for some examples.
-      *
-      * \sa class IOFormat, class WithFormat
-      */
-    inline const WithFormat<Derived> format(const IOFormat& fmt) const
-    {
-      return WithFormat<Derived>(derived(), fmt);
-    }
-
-    /** \returns the unique coefficient of a 1x1 expression */
-    EIGEN_DEVICE_FUNC
-    CoeffReturnType value() const
-    {
-      EIGEN_STATIC_ASSERT_SIZE_1x1(Derived)
-      eigen_assert(this->rows() == 1 && this->cols() == 1);
-      return derived().coeff(0,0);
-    }
-
-    EIGEN_DEVICE_FUNC bool all() const;
-    EIGEN_DEVICE_FUNC bool any() const;
-    EIGEN_DEVICE_FUNC Index count() const;
-
-    typedef VectorwiseOp<Derived, Horizontal> RowwiseReturnType;
-    typedef const VectorwiseOp<const Derived, Horizontal> ConstRowwiseReturnType;
-    typedef VectorwiseOp<Derived, Vertical> ColwiseReturnType;
-    typedef const VectorwiseOp<const Derived, Vertical> ConstColwiseReturnType;
-
-    /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations
-    *
-    * Example: \include MatrixBase_rowwise.cpp
-    * Output: \verbinclude MatrixBase_rowwise.out
-    *
-    * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
-    */
-    //Code moved here due to a CUDA compiler bug
-    EIGEN_DEVICE_FUNC inline ConstRowwiseReturnType rowwise() const {
-      return ConstRowwiseReturnType(derived());
-    }
-    EIGEN_DEVICE_FUNC RowwiseReturnType rowwise();
-
-    /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations
-    *
-    * Example: \include MatrixBase_colwise.cpp
-    * Output: \verbinclude MatrixBase_colwise.out
-    *
-    * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
-    */
-    EIGEN_DEVICE_FUNC inline ConstColwiseReturnType colwise() const {
-      return ConstColwiseReturnType(derived());
-    }
-    EIGEN_DEVICE_FUNC ColwiseReturnType colwise();
-
-    typedef CwiseNullaryOp<internal::scalar_random_op<Scalar>,PlainObject> RandomReturnType;
-    static const RandomReturnType Random(Index rows, Index cols);
-    static const RandomReturnType Random(Index size);
-    static const RandomReturnType Random();
-
-    template<typename ThenDerived,typename ElseDerived>
-    const Select<Derived,ThenDerived,ElseDerived>
-    select(const DenseBase<ThenDerived>& thenMatrix,
-           const DenseBase<ElseDerived>& elseMatrix) const;
-
-    template<typename ThenDerived>
-    inline const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
-    select(const DenseBase<ThenDerived>& thenMatrix, const typename ThenDerived::Scalar& elseScalar) const;
-
-    template<typename ElseDerived>
-    inline const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
-    select(const typename ElseDerived::Scalar& thenScalar, const DenseBase<ElseDerived>& elseMatrix) const;
-
-    template<int p> RealScalar lpNorm() const;
-
-    template<int RowFactor, int ColFactor>
-    EIGEN_DEVICE_FUNC
-    const Replicate<Derived,RowFactor,ColFactor> replicate() const;
-    /**
-    * \return an expression of the replication of \c *this
-    *
-    * Example: \include MatrixBase_replicate_int_int.cpp
-    * Output: \verbinclude MatrixBase_replicate_int_int.out
-    *
-    * \sa VectorwiseOp::replicate(), DenseBase::replicate<int,int>(), class Replicate
-    */
-    //Code moved here due to a CUDA compiler bug
-    EIGEN_DEVICE_FUNC
-    const Replicate<Derived, Dynamic, Dynamic> replicate(Index rowFactor, Index colFactor) const
-    {
-      return Replicate<Derived, Dynamic, Dynamic>(derived(), rowFactor, colFactor);
-    }
-
-    typedef Reverse<Derived, BothDirections> ReverseReturnType;
-    typedef const Reverse<const Derived, BothDirections> ConstReverseReturnType;
-    EIGEN_DEVICE_FUNC ReverseReturnType reverse();
-    /** This is the const version of reverse(). */
-    //Code moved here due to a CUDA compiler bug
-    EIGEN_DEVICE_FUNC ConstReverseReturnType reverse() const
-    {
-      return ConstReverseReturnType(derived());
-    }
-    EIGEN_DEVICE_FUNC void reverseInPlace();
-
-#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::DenseBase
-#define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-#define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
-#   include "../plugins/BlockMethods.h"
-#   ifdef EIGEN_DENSEBASE_PLUGIN
-#     include EIGEN_DENSEBASE_PLUGIN
-#   endif
-#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
-#undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
-#undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
-
-    // disable the use of evalTo for dense objects with a nice compilation error
-    template<typename Dest>
-    EIGEN_DEVICE_FUNC
-    inline void evalTo(Dest& ) const
-    {
-      EIGEN_STATIC_ASSERT((internal::is_same<Dest,void>::value),THE_EVAL_EVALTO_FUNCTION_SHOULD_NEVER_BE_CALLED_FOR_DENSE_OBJECTS);
-    }
-
-  protected:
-    /** Default constructor. Do nothing. */
-    EIGEN_DEVICE_FUNC DenseBase()
-    {
-      /* Just checks for self-consistency of the flags.
-       * Only do it when debugging Eigen, as this borders on paranoiac and could slow compilation down
-       */
-#ifdef EIGEN_INTERNAL_DEBUGGING
-      EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, int(IsRowMajor))
-                        && EIGEN_IMPLIES(MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1, int(!IsRowMajor))),
-                          INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION)
-#endif
-    }
-
-  private:
-    EIGEN_DEVICE_FUNC explicit DenseBase(int);
-    EIGEN_DEVICE_FUNC DenseBase(int,int);
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC explicit DenseBase(const DenseBase<OtherDerived>&);
-};
-
-} // end namespace Eigen
-
-#endif // EIGEN_DENSEBASE_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
deleted file mode 100644
index 668922f..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
+++ /dev/null
@@ -1,303 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2007-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
-// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MAPBASE_H
-#define EIGEN_MAPBASE_H
-
-#define EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived) \
-      EIGEN_STATIC_ASSERT((int(internal::evaluator<Derived>::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \
-                          YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT)
-
-namespace Eigen { 
-
-/** \ingroup Core_Module
-  *
-  * \brief Base class for dense Map and Block expression with direct access
-  *
-  * This base class provides the const low-level accessors (e.g. coeff, coeffRef) of dense
-  * Map and Block objects with direct access.
-  * Typical users do not have to directly deal with this class.
-  *
-  * This class can be extended by through the macro plugin \c EIGEN_MAPBASE_PLUGIN.
-  * See \link TopicCustomizing_Plugins customizing Eigen \endlink for details.
-  *
-  * The \c Derived class has to provide the following two methods describing the memory layout:
-  *  \code Index innerStride() const; \endcode
-  *  \code Index outerStride() const; \endcode
-  *
-  * \sa class Map, class Block
-  */
-template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
-  : public internal::dense_xpr_base<Derived>::type
-{
-  public:
-
-    typedef typename internal::dense_xpr_base<Derived>::type Base;
-    enum {
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-      InnerStrideAtCompileTime = internal::traits<Derived>::InnerStrideAtCompileTime,
-      SizeAtCompileTime = Base::SizeAtCompileTime
-    };
-
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef typename internal::conditional<
-                         bool(internal::is_lvalue<Derived>::value),
-                         Scalar *,
-                         const Scalar *>::type
-                     PointerType;
-
-    using Base::derived;
-//    using Base::RowsAtCompileTime;
-//    using Base::ColsAtCompileTime;
-//    using Base::SizeAtCompileTime;
-    using Base::MaxRowsAtCompileTime;
-    using Base::MaxColsAtCompileTime;
-    using Base::MaxSizeAtCompileTime;
-    using Base::IsVectorAtCompileTime;
-    using Base::Flags;
-    using Base::IsRowMajor;
-
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
-    using Base::lazyAssign;
-    using Base::eval;
-
-    using Base::innerStride;
-    using Base::outerStride;
-    using Base::rowStride;
-    using Base::colStride;
-
-    // bug 217 - compile error on ICC 11.1
-    using Base::operator=;
-
-    typedef typename Base::CoeffReturnType CoeffReturnType;
-
-    /** \copydoc DenseBase::rows() */
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_rows.value(); }
-    /** \copydoc DenseBase::cols() */
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_cols.value(); }
-
-    /** Returns a pointer to the first coefficient of the matrix or vector.
-      *
-      * \note When addressing this data, make sure to honor the strides returned by innerStride() and outerStride().
-      *
-      * \sa innerStride(), outerStride()
-      */
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return m_data; }
-
-    /** \copydoc PlainObjectBase::coeff(Index,Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeff(Index rowId, Index colId) const
-    {
-      return m_data[colId * colStride() + rowId * rowStride()];
-    }
-
-    /** \copydoc PlainObjectBase::coeff(Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeff(Index index) const
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return m_data[index * innerStride()];
-    }
-
-    /** \copydoc PlainObjectBase::coeffRef(Index,Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      return this->m_data[colId * colStride() + rowId * rowStride()];
-    }
-
-    /** \copydoc PlainObjectBase::coeffRef(Index) const */
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index index) const
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return this->m_data[index * innerStride()];
-    }
-
-    /** \internal */
-    template<int LoadMode>
-    inline PacketScalar packet(Index rowId, Index colId) const
-    {
-      return internal::ploadt<PacketScalar, LoadMode>
-               (m_data + (colId * colStride() + rowId * rowStride()));
-    }
-
-    /** \internal */
-    template<int LoadMode>
-    inline PacketScalar packet(Index index) const
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return internal::ploadt<PacketScalar, LoadMode>(m_data + index * innerStride());
-    }
-
-    /** \internal Constructor for fixed size matrices or vectors */
-    EIGEN_DEVICE_FUNC
-    explicit inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime)
-    {
-      EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
-      checkSanity<Derived>();
-    }
-
-    /** \internal Constructor for dynamically sized vectors */
-    EIGEN_DEVICE_FUNC
-    inline MapBase(PointerType dataPtr, Index vecSize)
-            : m_data(dataPtr),
-              m_rows(RowsAtCompileTime == Dynamic ? vecSize : Index(RowsAtCompileTime)),
-              m_cols(ColsAtCompileTime == Dynamic ? vecSize : Index(ColsAtCompileTime))
-    {
-      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-      eigen_assert(vecSize >= 0);
-      eigen_assert(dataPtr == 0 || SizeAtCompileTime == Dynamic || SizeAtCompileTime == vecSize);
-      checkSanity<Derived>();
-    }
-
-    /** \internal Constructor for dynamically sized matrices */
-    EIGEN_DEVICE_FUNC
-    inline MapBase(PointerType dataPtr, Index rows, Index cols)
-            : m_data(dataPtr), m_rows(rows), m_cols(cols)
-    {
-      eigen_assert( (dataPtr == 0)
-              || (   rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
-                  && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)));
-      checkSanity<Derived>();
-    }
-
-    #ifdef EIGEN_MAPBASE_PLUGIN
-    #include EIGEN_MAPBASE_PLUGIN
-    #endif
-
-  protected:
-
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    void checkSanity(typename internal::enable_if<(internal::traits<T>::Alignment>0),void*>::type = 0) const
-    {
-#if EIGEN_MAX_ALIGN_BYTES>0
-      // innerStride() is not set yet when this function is called, so we optimistically assume the lowest plausible value:
-      const Index minInnerStride = InnerStrideAtCompileTime == Dynamic ? 1 : Index(InnerStrideAtCompileTime);
-      EIGEN_ONLY_USED_FOR_DEBUG(minInnerStride);
-      eigen_assert((   ((internal::UIntPtr(m_data) % internal::traits<Derived>::Alignment) == 0)
-                    || (cols() * rows() * minInnerStride * sizeof(Scalar)) < internal::traits<Derived>::Alignment ) && "data is not aligned");
-#endif
-    }
-
-    template<typename T>
-    EIGEN_DEVICE_FUNC
-    void checkSanity(typename internal::enable_if<internal::traits<T>::Alignment==0,void*>::type = 0) const
-    {}
-
-    PointerType m_data;
-    const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_rows;
-    const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_cols;
-};
-
-/** \ingroup Core_Module
-  *
-  * \brief Base class for non-const dense Map and Block expression with direct access
-  *
-  * This base class provides the non-const low-level accessors (e.g. coeff and coeffRef) of
-  * dense Map and Block objects with direct access.
-  * It inherits MapBase<Derived, ReadOnlyAccessors> which defines the const variant for reading specific entries.
-  *
-  * \sa class Map, class Block
-  */
-template<typename Derived> class MapBase<Derived, WriteAccessors>
-  : public MapBase<Derived, ReadOnlyAccessors>
-{
-    typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
-  public:
-
-    typedef MapBase<Derived, ReadOnlyAccessors> Base;
-
-    typedef typename Base::Scalar Scalar;
-    typedef typename Base::PacketScalar PacketScalar;
-    typedef typename Base::StorageIndex StorageIndex;
-    typedef typename Base::PointerType PointerType;
-
-    using Base::derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
-
-    using Base::innerStride;
-    using Base::outerStride;
-    using Base::rowStride;
-    using Base::colStride;
-
-    typedef typename internal::conditional<
-                    internal::is_lvalue<Derived>::value,
-                    Scalar,
-                    const Scalar
-                  >::type ScalarWithConstIfNotLvalue;
-
-    EIGEN_DEVICE_FUNC
-    inline const Scalar* data() const { return this->m_data; }
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue* data() { return this->m_data; } // no const-cast here so non-const-correct code will give a compile error
-
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue& coeffRef(Index row, Index col)
-    {
-      return this->m_data[col * colStride() + row * rowStride()];
-    }
-
-    EIGEN_DEVICE_FUNC
-    inline ScalarWithConstIfNotLvalue& coeffRef(Index index)
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      return this->m_data[index * innerStride()];
-    }
-
-    template<int StoreMode>
-    inline void writePacket(Index row, Index col, const PacketScalar& val)
-    {
-      internal::pstoret<Scalar, PacketScalar, StoreMode>
-               (this->m_data + (col * colStride() + row * rowStride()), val);
-    }
-
-    template<int StoreMode>
-    inline void writePacket(Index index, const PacketScalar& val)
-    {
-      EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS(Derived)
-      internal::pstoret<Scalar, PacketScalar, StoreMode>
-                (this->m_data + index * innerStride(), val);
-    }
-
-    EIGEN_DEVICE_FUNC explicit inline MapBase(PointerType dataPtr) : Base(dataPtr) {}
-    EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index vecSize) : Base(dataPtr, vecSize) {}
-    EIGEN_DEVICE_FUNC inline MapBase(PointerType dataPtr, Index rows, Index cols) : Base(dataPtr, rows, cols) {}
-
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const MapBase& other)
-    {
-      ReadOnlyMapBase::Base::operator=(other);
-      return derived();
-    }
-
-    // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base,
-    // see bugs 821 and 920.
-    using ReadOnlyMapBase::Base::operator=;
-};
-
-#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS
-
-} // end namespace Eigen
-
-#endif // EIGEN_MAPBASE_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
deleted file mode 100644
index e6c3590..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
+++ /dev/null
@@ -1,529 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MATRIXBASE_H
-#define EIGEN_MATRIXBASE_H
-
-namespace Eigen {
-
-/** \class MatrixBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for all dense matrices, vectors, and expressions
-  *
-  * This class is the base that is inherited by all matrix, vector, and related expression
-  * types. Most of the Eigen API is contained in this class, and its base classes. Other important
-  * classes for the Eigen API are Matrix, and VectorwiseOp.
-  *
-  * Note that some methods are defined in other modules such as the \ref LU_Module LU module
-  * for all functions related to matrix inversions.
-  *
-  * \tparam Derived is the derived type, e.g. a matrix type, or an expression, etc.
-  *
-  * When writing a function taking Eigen objects as argument, if you want your function
-  * to take as argument any matrix, vector, or expression, just let it take a
-  * MatrixBase argument. As an example, here is a function printFirstRow which, given
-  * a matrix, vector, or expression \a x, prints the first row of \a x.
-  *
-  * \code
-    template<typename Derived>
-    void printFirstRow(const Eigen::MatrixBase<Derived>& x)
-    {
-      cout << x.row(0) << endl;
-    }
-  * \endcode
-  *
-  * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_MATRIXBASE_PLUGIN.
-  *
-  * \sa \blank \ref TopicClassHierarchy
-  */
-template<typename Derived> class MatrixBase
-  : public DenseBase<Derived>
-{
-  public:
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    typedef MatrixBase StorageBaseType;
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::packet_traits<Scalar>::type PacketScalar;
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-
-    typedef DenseBase<Derived> Base;
-    using Base::RowsAtCompileTime;
-    using Base::ColsAtCompileTime;
-    using Base::SizeAtCompileTime;
-    using Base::MaxRowsAtCompileTime;
-    using Base::MaxColsAtCompileTime;
-    using Base::MaxSizeAtCompileTime;
-    using Base::IsVectorAtCompileTime;
-    using Base::Flags;
-
-    using Base::derived;
-    using Base::const_cast_derived;
-    using Base::rows;
-    using Base::cols;
-    using Base::size;
-    using Base::coeff;
-    using Base::coeffRef;
-    using Base::lazyAssign;
-    using Base::eval;
-    using Base::operator+=;
-    using Base::operator-=;
-    using Base::operator*=;
-    using Base::operator/=;
-
-    typedef typename Base::CoeffReturnType CoeffReturnType;
-    typedef typename Base::ConstTransposeReturnType ConstTransposeReturnType;
-    typedef typename Base::RowXpr RowXpr;
-    typedef typename Base::ColXpr ColXpr;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** type of the equivalent square matrix */
-    typedef Matrix<Scalar,EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime),
-                          EIGEN_SIZE_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-    /** \returns the size of the main diagonal, which is min(rows(),cols()).
-      * \sa rows(), cols(), SizeAtCompileTime. */
-    EIGEN_DEVICE_FUNC
-    inline Index diagonalSize() const { return (numext::mini)(rows(),cols()); }
-
-    typedef typename Base::PlainObject PlainObject;
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** \internal Represents a matrix with all coefficients equal to one another*/
-    typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
-    /** \internal the return type of MatrixBase::adjoint() */
-    typedef typename internal::conditional<NumTraits<Scalar>::IsComplex,
-                        CwiseUnaryOp<internal::scalar_conjugate_op<Scalar>, ConstTransposeReturnType>,
-                        ConstTransposeReturnType
-                     >::type AdjointReturnType;
-    /** \internal Return type of eigenvalues() */
-    typedef Matrix<std::complex<RealScalar>, internal::traits<Derived>::ColsAtCompileTime, 1, ColMajor> EigenvaluesReturnType;
-    /** \internal the return type of identity */
-    typedef CwiseNullaryOp<internal::scalar_identity_op<Scalar>,PlainObject> IdentityReturnType;
-    /** \internal the return type of unit vectors */
-    typedef Block<const CwiseNullaryOp<internal::scalar_identity_op<Scalar>, SquareMatrixType>,
-                  internal::traits<Derived>::RowsAtCompileTime,
-                  internal::traits<Derived>::ColsAtCompileTime> BasisReturnType;
-#endif // not EIGEN_PARSED_BY_DOXYGEN
-
-#define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::MatrixBase
-#define EIGEN_DOC_UNARY_ADDONS(X,Y)
-#   include "../plugins/CommonCwiseUnaryOps.h"
-#   include "../plugins/CommonCwiseBinaryOps.h"
-#   include "../plugins/MatrixCwiseUnaryOps.h"
-#   include "../plugins/MatrixCwiseBinaryOps.h"
-#   ifdef EIGEN_MATRIXBASE_PLUGIN
-#     include EIGEN_MATRIXBASE_PLUGIN
-#   endif
-#undef EIGEN_CURRENT_STORAGE_BASE_CLASS
-#undef EIGEN_DOC_UNARY_ADDONS
-
-    /** Special case of the template operator=, in order to prevent the compiler
-      * from generating a default operator= (issue hit with g++ 4.1)
-      */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const MatrixBase& other);
-
-    // We cannot inherit here via Base::operator= since it is causing
-    // trouble with MSVC.
-
-    template <typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator=(const DenseBase<OtherDerived>& other);
-
-    template <typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const EigenBase<OtherDerived>& other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    Derived& operator=(const ReturnByValue<OtherDerived>& other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator+=(const MatrixBase<OtherDerived>& other);
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Derived& operator-=(const MatrixBase<OtherDerived>& other);
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<Derived,OtherDerived>
-    operator*(const MatrixBase<OtherDerived> &other) const;
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<Derived,OtherDerived,LazyProduct>
-    lazyProduct(const MatrixBase<OtherDerived> &other) const;
-
-    template<typename OtherDerived>
-    Derived& operator*=(const EigenBase<OtherDerived>& other);
-
-    template<typename OtherDerived>
-    void applyOnTheLeft(const EigenBase<OtherDerived>& other);
-
-    template<typename OtherDerived>
-    void applyOnTheRight(const EigenBase<OtherDerived>& other);
-
-    template<typename DiagonalDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<Derived, DiagonalDerived, LazyProduct>
-    operator*(const DiagonalBase<DiagonalDerived> &diagonal) const;
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
-    dot(const MatrixBase<OtherDerived>& other) const;
-
-    EIGEN_DEVICE_FUNC RealScalar squaredNorm() const;
-    EIGEN_DEVICE_FUNC RealScalar norm() const;
-    RealScalar stableNorm() const;
-    RealScalar blueNorm() const;
-    RealScalar hypotNorm() const;
-    EIGEN_DEVICE_FUNC const PlainObject normalized() const;
-    EIGEN_DEVICE_FUNC const PlainObject stableNormalized() const;
-    EIGEN_DEVICE_FUNC void normalize();
-    EIGEN_DEVICE_FUNC void stableNormalize();
-
-    EIGEN_DEVICE_FUNC const AdjointReturnType adjoint() const;
-    EIGEN_DEVICE_FUNC void adjointInPlace();
-
-    typedef Diagonal<Derived> DiagonalReturnType;
-    EIGEN_DEVICE_FUNC
-    DiagonalReturnType diagonal();
-
-    typedef typename internal::add_const<Diagonal<const Derived> >::type ConstDiagonalReturnType;
-    EIGEN_DEVICE_FUNC
-    ConstDiagonalReturnType diagonal() const;
-
-    template<int Index> struct DiagonalIndexReturnType { typedef Diagonal<Derived,Index> Type; };
-    template<int Index> struct ConstDiagonalIndexReturnType { typedef const Diagonal<const Derived,Index> Type; };
-
-    template<int Index>
-    EIGEN_DEVICE_FUNC
-    typename DiagonalIndexReturnType<Index>::Type diagonal();
-
-    template<int Index>
-    EIGEN_DEVICE_FUNC
-    typename ConstDiagonalIndexReturnType<Index>::Type diagonal() const;
-
-    typedef Diagonal<Derived,DynamicIndex> DiagonalDynamicIndexReturnType;
-    typedef typename internal::add_const<Diagonal<const Derived,DynamicIndex> >::type ConstDiagonalDynamicIndexReturnType;
-
-    EIGEN_DEVICE_FUNC
-    DiagonalDynamicIndexReturnType diagonal(Index index);
-    EIGEN_DEVICE_FUNC
-    ConstDiagonalDynamicIndexReturnType diagonal(Index index) const;
-
-    template<unsigned int Mode> struct TriangularViewReturnType { typedef TriangularView<Derived, Mode> Type; };
-    template<unsigned int Mode> struct ConstTriangularViewReturnType { typedef const TriangularView<const Derived, Mode> Type; };
-
-    template<unsigned int Mode>
-    EIGEN_DEVICE_FUNC
-    typename TriangularViewReturnType<Mode>::Type triangularView();
-    template<unsigned int Mode>
-    EIGEN_DEVICE_FUNC
-    typename ConstTriangularViewReturnType<Mode>::Type triangularView() const;
-
-    template<unsigned int UpLo> struct SelfAdjointViewReturnType { typedef SelfAdjointView<Derived, UpLo> Type; };
-    template<unsigned int UpLo> struct ConstSelfAdjointViewReturnType { typedef const SelfAdjointView<const Derived, UpLo> Type; };
-
-    template<unsigned int UpLo>
-    EIGEN_DEVICE_FUNC
-    typename SelfAdjointViewReturnType<UpLo>::Type selfadjointView();
-    template<unsigned int UpLo>
-    EIGEN_DEVICE_FUNC
-    typename ConstSelfAdjointViewReturnType<UpLo>::Type selfadjointView() const;
-
-    const SparseView<Derived> sparseView(const Scalar& m_reference = Scalar(0),
-                                         const typename NumTraits<Scalar>::Real& m_epsilon = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC static const IdentityReturnType Identity();
-    EIGEN_DEVICE_FUNC static const IdentityReturnType Identity(Index rows, Index cols);
-    EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index size, Index i);
-    EIGEN_DEVICE_FUNC static const BasisReturnType Unit(Index i);
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitX();
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitY();
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitZ();
-    EIGEN_DEVICE_FUNC static const BasisReturnType UnitW();
-
-    EIGEN_DEVICE_FUNC
-    const DiagonalWrapper<const Derived> asDiagonal() const;
-    const PermutationWrapper<const Derived> asPermutation() const;
-
-    EIGEN_DEVICE_FUNC
-    Derived& setIdentity();
-    EIGEN_DEVICE_FUNC
-    Derived& setIdentity(Index rows, Index cols);
-
-    bool isIdentity(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    bool isDiagonal(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-
-    bool isUpperTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    bool isLowerTriangular(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-
-    template<typename OtherDerived>
-    bool isOrthogonal(const MatrixBase<OtherDerived>& other,
-                      const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    bool isUnitary(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-
-    /** \returns true if each coefficients of \c *this and \a other are all exactly equal.
-      * \warning When using floating point scalar values you probably should rather use a
-      *          fuzzy comparison such as isApprox()
-      * \sa isApprox(), operator!= */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC inline bool operator==(const MatrixBase<OtherDerived>& other) const
-    { return cwiseEqual(other).all(); }
-
-    /** \returns true if at least one pair of coefficients of \c *this and \a other are not exactly equal to each other.
-      * \warning When using floating point scalar values you probably should rather use a
-      *          fuzzy comparison such as isApprox()
-      * \sa isApprox(), operator== */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC inline bool operator!=(const MatrixBase<OtherDerived>& other) const
-    { return cwiseNotEqual(other).any(); }
-
-    NoAlias<Derived,Eigen::MatrixBase > noalias();
-
-    // TODO forceAlignedAccess is temporarily disabled
-    // Need to find a nicer workaround.
-    inline const Derived& forceAlignedAccess() const { return derived(); }
-    inline Derived& forceAlignedAccess() { return derived(); }
-    template<bool Enable> inline const Derived& forceAlignedAccessIf() const { return derived(); }
-    template<bool Enable> inline Derived& forceAlignedAccessIf() { return derived(); }
-
-    EIGEN_DEVICE_FUNC Scalar trace() const;
-
-    template<int p> EIGEN_DEVICE_FUNC RealScalar lpNorm() const;
-
-    EIGEN_DEVICE_FUNC MatrixBase<Derived>& matrix() { return *this; }
-    EIGEN_DEVICE_FUNC const MatrixBase<Derived>& matrix() const { return *this; }
-
-    /** \returns an \link Eigen::ArrayBase Array \endlink expression of this matrix
-      * \sa ArrayBase::matrix() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ArrayWrapper<Derived> array() { return ArrayWrapper<Derived>(derived()); }
-    /** \returns a const \link Eigen::ArrayBase Array \endlink expression of this matrix
-      * \sa ArrayBase::matrix() */
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const ArrayWrapper<const Derived> array() const { return ArrayWrapper<const Derived>(derived()); }
-
-/////////// LU module ///////////
-
-    inline const FullPivLU<PlainObject> fullPivLu() const;
-    inline const PartialPivLU<PlainObject> partialPivLu() const;
-
-    inline const PartialPivLU<PlainObject> lu() const;
-
-    inline const Inverse<Derived> inverse() const;
-
-    template<typename ResultType>
-    inline void computeInverseAndDetWithCheck(
-      ResultType& inverse,
-      typename ResultType::Scalar& determinant,
-      bool& invertible,
-      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
-    ) const;
-    template<typename ResultType>
-    inline void computeInverseWithCheck(
-      ResultType& inverse,
-      bool& invertible,
-      const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
-    ) const;
-    Scalar determinant() const;
-
-/////////// Cholesky module ///////////
-
-    inline const LLT<PlainObject>  llt() const;
-    inline const LDLT<PlainObject> ldlt() const;
-
-/////////// QR module ///////////
-
-    inline const HouseholderQR<PlainObject> householderQr() const;
-    inline const ColPivHouseholderQR<PlainObject> colPivHouseholderQr() const;
-    inline const FullPivHouseholderQR<PlainObject> fullPivHouseholderQr() const;
-    inline const CompleteOrthogonalDecomposition<PlainObject> completeOrthogonalDecomposition() const;
-
-/////////// Eigenvalues module ///////////
-
-    inline EigenvaluesReturnType eigenvalues() const;
-    inline RealScalar operatorNorm() const;
-
-/////////// SVD module ///////////
-
-    inline JacobiSVD<PlainObject> jacobiSvd(unsigned int computationOptions = 0) const;
-    inline BDCSVD<PlainObject>    bdcSvd(unsigned int computationOptions = 0) const;
-
-/////////// Geometry module ///////////
-
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /// \internal helper struct to form the return type of the cross product
-    template<typename OtherDerived> struct cross_product_return_type {
-      typedef typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType Scalar;
-      typedef Matrix<Scalar,MatrixBase::RowsAtCompileTime,MatrixBase::ColsAtCompileTime> type;
-    };
-    #endif // EIGEN_PARSED_BY_DOXYGEN
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    inline typename cross_product_return_type<OtherDerived>::type
-#else
-    inline PlainObject
-#endif
-    cross(const MatrixBase<OtherDerived>& other) const;
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    inline PlainObject cross3(const MatrixBase<OtherDerived>& other) const;
-
-    EIGEN_DEVICE_FUNC
-    inline PlainObject unitOrthogonal(void) const;
-
-    EIGEN_DEVICE_FUNC
-    inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2) const;
-
-    // put this as separate enum value to work around possible GCC 4.3 bug (?)
-    enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits<Derived>::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical)
-                                          : ColsAtCompileTime==1 ? Vertical : Horizontal };
-    typedef Homogeneous<Derived, HomogeneousReturnTypeDirection> HomogeneousReturnType;
-    EIGEN_DEVICE_FUNC
-    inline HomogeneousReturnType homogeneous() const;
-
-    enum {
-      SizeMinusOne = SizeAtCompileTime==Dynamic ? Dynamic : SizeAtCompileTime-1
-    };
-    typedef Block<const Derived,
-                  internal::traits<Derived>::ColsAtCompileTime==1 ? SizeMinusOne : 1,
-                  internal::traits<Derived>::ColsAtCompileTime==1 ? 1 : SizeMinusOne> ConstStartMinusOne;
-    typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(ConstStartMinusOne,Scalar,quotient) HNormalizedReturnType;
-    EIGEN_DEVICE_FUNC
-    inline const HNormalizedReturnType hnormalized() const;
-
-////////// Householder module ///////////
-
-    void makeHouseholderInPlace(Scalar& tau, RealScalar& beta);
-    template<typename EssentialPart>
-    void makeHouseholder(EssentialPart& essential,
-                         Scalar& tau, RealScalar& beta) const;
-    template<typename EssentialPart>
-    void applyHouseholderOnTheLeft(const EssentialPart& essential,
-                                   const Scalar& tau,
-                                   Scalar* workspace);
-    template<typename EssentialPart>
-    void applyHouseholderOnTheRight(const EssentialPart& essential,
-                                    const Scalar& tau,
-                                    Scalar* workspace);
-
-///////// Jacobi module /////////
-
-    template<typename OtherScalar>
-    void applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j);
-    template<typename OtherScalar>
-    void applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j);
-
-///////// SparseCore module /////////
-
-    template<typename OtherDerived>
-    EIGEN_STRONG_INLINE const typename SparseMatrixBase<OtherDerived>::template CwiseProductDenseReturnType<Derived>::Type
-    cwiseProduct(const SparseMatrixBase<OtherDerived> &other) const
-    {
-      return other.cwiseProduct(derived());
-    }
-
-///////// MatrixFunctions module /////////
-
-    typedef typename internal::stem_function<Scalar>::type StemFunction;
-#define EIGEN_MATRIX_FUNCTION(ReturnType, Name, Description) \
-    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
-    const ReturnType<Derived> Name() const;
-#define EIGEN_MATRIX_FUNCTION_1(ReturnType, Name, Description, Argument) \
-    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
-    const ReturnType<Derived> Name(Argument) const;
-
-    EIGEN_MATRIX_FUNCTION(MatrixExponentialReturnValue, exp, exponential)
-    /** \brief Helper function for the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>.*/
-    const MatrixFunctionReturnValue<Derived> matrixFunction(StemFunction f) const;
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cosh, hyperbolic cosine)
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sinh, hyperbolic sine)
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cos, cosine)
-    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sin, sine)
-    EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root)
-    EIGEN_MATRIX_FUNCTION(MatrixLogarithmReturnValue, log, logarithm)
-    EIGEN_MATRIX_FUNCTION_1(MatrixPowerReturnValue,        pow, power to \c p, const RealScalar& p)
-    EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex<RealScalar>& p)
-
-  protected:
-    EIGEN_DEVICE_FUNC MatrixBase() : Base() {}
-
-  private:
-    EIGEN_DEVICE_FUNC explicit MatrixBase(int);
-    EIGEN_DEVICE_FUNC MatrixBase(int,int);
-    template<typename OtherDerived> EIGEN_DEVICE_FUNC explicit MatrixBase(const MatrixBase<OtherDerived>&);
-  protected:
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator+=(const ArrayBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
-    // mixing arrays and matrices is not legal
-    template<typename OtherDerived> Derived& operator-=(const ArrayBase<OtherDerived>& )
-    {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
-};
-
-
-/***************************************************************************
-* Implementation of matrix base methods
-***************************************************************************/
-
-/** replaces \c *this by \c *this * \a other.
-  *
-  * \returns a reference to \c *this
-  *
-  * Example: \include MatrixBase_applyOnTheRight.cpp
-  * Output: \verbinclude MatrixBase_applyOnTheRight.out
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline Derived&
-MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived> &other)
-{
-  other.derived().applyThisOnTheRight(derived());
-  return derived();
-}
-
-/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=().
-  *
-  * Example: \include MatrixBase_applyOnTheRight.cpp
-  * Output: \verbinclude MatrixBase_applyOnTheRight.out
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived> &other)
-{
-  other.derived().applyThisOnTheRight(derived());
-}
-
-/** replaces \c *this by \a other * \c *this.
-  *
-  * Example: \include MatrixBase_applyOnTheLeft.cpp
-  * Output: \verbinclude MatrixBase_applyOnTheLeft.out
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived> &other)
-{
-  other.derived().applyThisOnTheLeft(derived());
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_MATRIXBASE_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
deleted file mode 100644
index 79b767b..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
+++ /dev/null
@@ -1,403 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-// Copyright (C) 2009-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_TRANSPOSE_H
-#define EIGEN_TRANSPOSE_H
-
-namespace Eigen { 
-
-namespace internal {
-template<typename MatrixType>
-struct traits<Transpose<MatrixType> > : public traits<MatrixType>
-{
-  typedef typename ref_selector<MatrixType>::type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type MatrixTypeNestedPlain;
-  enum {
-    RowsAtCompileTime = MatrixType::ColsAtCompileTime,
-    ColsAtCompileTime = MatrixType::RowsAtCompileTime,
-    MaxRowsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    MaxColsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
-    FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
-    Flags0 = traits<MatrixTypeNestedPlain>::Flags & ~(LvalueBit | NestByRefBit),
-    Flags1 = Flags0 | FlagsLvalueBit,
-    Flags = Flags1 ^ RowMajorBit,
-    InnerStrideAtCompileTime = inner_stride_at_compile_time<MatrixType>::ret,
-    OuterStrideAtCompileTime = outer_stride_at_compile_time<MatrixType>::ret
-  };
-};
-}
-
-template<typename MatrixType, typename StorageKind> class TransposeImpl;
-
-/** \class Transpose
-  * \ingroup Core_Module
-  *
-  * \brief Expression of the transpose of a matrix
-  *
-  * \tparam MatrixType the type of the object of which we are taking the transpose
-  *
-  * This class represents an expression of the transpose of a matrix.
-  * It is the return type of MatrixBase::transpose() and MatrixBase::adjoint()
-  * and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::transpose(), MatrixBase::adjoint()
-  */
-template<typename MatrixType> class Transpose
-  : public TransposeImpl<MatrixType,typename internal::traits<MatrixType>::StorageKind>
-{
-  public:
-
-    typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
-
-    typedef typename TransposeImpl<MatrixType,typename internal::traits<MatrixType>::StorageKind>::Base Base;
-    EIGEN_GENERIC_PUBLIC_INTERFACE(Transpose)
-    typedef typename internal::remove_all<MatrixType>::type NestedExpression;
-
-    EIGEN_DEVICE_FUNC
-    explicit inline Transpose(MatrixType& matrix) : m_matrix(matrix) {}
-
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Transpose)
-
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_matrix.cols(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.rows(); }
-
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC
-    const typename internal::remove_all<MatrixTypeNested>::type&
-    nestedExpression() const { return m_matrix; }
-
-    /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC
-    typename internal::remove_reference<MatrixTypeNested>::type&
-    nestedExpression() { return m_matrix; }
-
-    /** \internal */
-    void resize(Index nrows, Index ncols) {
-      m_matrix.resize(ncols,nrows);
-    }
-
-  protected:
-    typename internal::ref_selector<MatrixType>::non_const_type m_matrix;
-};
-
-namespace internal {
-
-template<typename MatrixType, bool HasDirectAccess = has_direct_access<MatrixType>::ret>
-struct TransposeImpl_base
-{
-  typedef typename dense_xpr_base<Transpose<MatrixType> >::type type;
-};
-
-template<typename MatrixType>
-struct TransposeImpl_base<MatrixType, false>
-{
-  typedef typename dense_xpr_base<Transpose<MatrixType> >::type type;
-};
-
-} // end namespace internal
-
-// Generic API dispatcher
-template<typename XprType, typename StorageKind>
-class TransposeImpl
-  : public internal::generic_xpr_base<Transpose<XprType> >::type
-{
-public:
-  typedef typename internal::generic_xpr_base<Transpose<XprType> >::type Base;
-};
-
-template<typename MatrixType> class TransposeImpl<MatrixType,Dense>
-  : public internal::TransposeImpl_base<MatrixType>::type
-{
-  public:
-
-    typedef typename internal::TransposeImpl_base<MatrixType>::type Base;
-    using Base::coeffRef;
-    EIGEN_DENSE_PUBLIC_INTERFACE(Transpose<MatrixType>)
-    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TransposeImpl)
-
-    EIGEN_DEVICE_FUNC inline Index innerStride() const { return derived().nestedExpression().innerStride(); }
-    EIGEN_DEVICE_FUNC inline Index outerStride() const { return derived().nestedExpression().outerStride(); }
-
-    typedef typename internal::conditional<
-                       internal::is_lvalue<MatrixType>::value,
-                       Scalar,
-                       const Scalar
-                     >::type ScalarWithConstIfNotLvalue;
-
-    EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return derived().nestedExpression().data(); }
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return derived().nestedExpression().data(); }
-
-    // FIXME: shall we keep the const version of coeffRef?
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index rowId, Index colId) const
-    {
-      return derived().nestedExpression().coeffRef(colId, rowId);
-    }
-
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index index) const
-    {
-      return derived().nestedExpression().coeffRef(index);
-    }
-};
-
-/** \returns an expression of the transpose of *this.
-  *
-  * Example: \include MatrixBase_transpose.cpp
-  * Output: \verbinclude MatrixBase_transpose.out
-  *
-  * \warning If you want to replace a matrix by its own transpose, do \b NOT do this:
-  * \code
-  * m = m.transpose(); // bug!!! caused by aliasing effect
-  * \endcode
-  * Instead, use the transposeInPlace() method:
-  * \code
-  * m.transposeInPlace();
-  * \endcode
-  * which gives Eigen good opportunities for optimization, or alternatively you can also do:
-  * \code
-  * m = m.transpose().eval();
-  * \endcode
-  *
-  * \sa transposeInPlace(), adjoint() */
-template<typename Derived>
-inline Transpose<Derived>
-DenseBase<Derived>::transpose()
-{
-  return TransposeReturnType(derived());
-}
-
-/** This is the const version of transpose().
-  *
-  * Make sure you read the warning for transpose() !
-  *
-  * \sa transposeInPlace(), adjoint() */
-template<typename Derived>
-inline typename DenseBase<Derived>::ConstTransposeReturnType
-DenseBase<Derived>::transpose() const
-{
-  return ConstTransposeReturnType(derived());
-}
-
-/** \returns an expression of the adjoint (i.e. conjugate transpose) of *this.
-  *
-  * Example: \include MatrixBase_adjoint.cpp
-  * Output: \verbinclude MatrixBase_adjoint.out
-  *
-  * \warning If you want to replace a matrix by its own adjoint, do \b NOT do this:
-  * \code
-  * m = m.adjoint(); // bug!!! caused by aliasing effect
-  * \endcode
-  * Instead, use the adjointInPlace() method:
-  * \code
-  * m.adjointInPlace();
-  * \endcode
-  * which gives Eigen good opportunities for optimization, or alternatively you can also do:
-  * \code
-  * m = m.adjoint().eval();
-  * \endcode
-  *
-  * \sa adjointInPlace(), transpose(), conjugate(), class Transpose, class internal::scalar_conjugate_op */
-template<typename Derived>
-inline const typename MatrixBase<Derived>::AdjointReturnType
-MatrixBase<Derived>::adjoint() const
-{
-  return AdjointReturnType(this->transpose());
-}
-
-/***************************************************************************
-* "in place" transpose implementation
-***************************************************************************/
-
-namespace internal {
-
-template<typename MatrixType,
-  bool IsSquare = (MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime) && MatrixType::RowsAtCompileTime!=Dynamic,
-  bool MatchPacketSize =
-        (int(MatrixType::RowsAtCompileTime) == int(internal::packet_traits<typename MatrixType::Scalar>::size))
-    &&  (internal::evaluator<MatrixType>::Flags&PacketAccessBit) >
-struct inplace_transpose_selector;
-
-template<typename MatrixType>
-struct inplace_transpose_selector<MatrixType,true,false> { // square matrix
-  static void run(MatrixType& m) {
-    m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose());
-  }
-};
-
-// TODO: vectorized path is currently limited to LargestPacketSize x LargestPacketSize cases only.
-template<typename MatrixType>
-struct inplace_transpose_selector<MatrixType,true,true> { // PacketSize x PacketSize
-  static void run(MatrixType& m) {
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename internal::packet_traits<typename MatrixType::Scalar>::type Packet;
-    const Index PacketSize = internal::packet_traits<Scalar>::size;
-    const Index Alignment = internal::evaluator<MatrixType>::Alignment;
-    PacketBlock<Packet> A;
-    for (Index i=0; i<PacketSize; ++i)
-      A.packet[i] = m.template packetByOuterInner<Alignment>(i,0);
-    internal::ptranspose(A);
-    for (Index i=0; i<PacketSize; ++i)
-      m.template writePacket<Alignment>(m.rowIndexByOuterInner(i,0), m.colIndexByOuterInner(i,0), A.packet[i]);
-  }
-};
-
-template<typename MatrixType,bool MatchPacketSize>
-struct inplace_transpose_selector<MatrixType,false,MatchPacketSize> { // non square matrix
-  static void run(MatrixType& m) {
-    if (m.rows()==m.cols())
-      m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose());
-    else
-      m = m.transpose().eval();
-  }
-};
-
-} // end namespace internal
-
-/** This is the "in place" version of transpose(): it replaces \c *this by its own transpose.
-  * Thus, doing
-  * \code
-  * m.transposeInPlace();
-  * \endcode
-  * has the same effect on m as doing
-  * \code
-  * m = m.transpose().eval();
-  * \endcode
-  * and is faster and also safer because in the latter line of code, forgetting the eval() results
-  * in a bug caused by \ref TopicAliasing "aliasing".
-  *
-  * Notice however that this method is only useful if you want to replace a matrix by its own transpose.
-  * If you just need the transpose of a matrix, use transpose().
-  *
-  * \note if the matrix is not square, then \c *this must be a resizable matrix. 
-  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
-  *
-  * \sa transpose(), adjoint(), adjointInPlace() */
-template<typename Derived>
-inline void DenseBase<Derived>::transposeInPlace()
-{
-  eigen_assert((rows() == cols() || (RowsAtCompileTime == Dynamic && ColsAtCompileTime == Dynamic))
-               && "transposeInPlace() called on a non-square non-resizable matrix");
-  internal::inplace_transpose_selector<Derived>::run(derived());
-}
-
-/***************************************************************************
-* "in place" adjoint implementation
-***************************************************************************/
-
-/** This is the "in place" version of adjoint(): it replaces \c *this by its own transpose.
-  * Thus, doing
-  * \code
-  * m.adjointInPlace();
-  * \endcode
-  * has the same effect on m as doing
-  * \code
-  * m = m.adjoint().eval();
-  * \endcode
-  * and is faster and also safer because in the latter line of code, forgetting the eval() results
-  * in a bug caused by aliasing.
-  *
-  * Notice however that this method is only useful if you want to replace a matrix by its own adjoint.
-  * If you just need the adjoint of a matrix, use adjoint().
-  *
-  * \note if the matrix is not square, then \c *this must be a resizable matrix.
-  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
-  *
-  * \sa transpose(), adjoint(), transposeInPlace() */
-template<typename Derived>
-inline void MatrixBase<Derived>::adjointInPlace()
-{
-  derived() = adjoint().eval();
-}
-
-#ifndef EIGEN_NO_DEBUG
-
-// The following is to detect aliasing problems in most common cases.
-
-namespace internal {
-
-template<bool DestIsTransposed, typename OtherDerived>
-struct check_transpose_aliasing_compile_time_selector
-{
-  enum { ret = bool(blas_traits<OtherDerived>::IsTransposed) != DestIsTransposed };
-};
-
-template<bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
-struct check_transpose_aliasing_compile_time_selector<DestIsTransposed,CwiseBinaryOp<BinOp,DerivedA,DerivedB> >
-{
-  enum { ret =    bool(blas_traits<DerivedA>::IsTransposed) != DestIsTransposed
-               || bool(blas_traits<DerivedB>::IsTransposed) != DestIsTransposed
-  };
-};
-
-template<typename Scalar, bool DestIsTransposed, typename OtherDerived>
-struct check_transpose_aliasing_run_time_selector
-{
-  static bool run(const Scalar* dest, const OtherDerived& src)
-  {
-    return (bool(blas_traits<OtherDerived>::IsTransposed) != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src));
-  }
-};
-
-template<typename Scalar, bool DestIsTransposed, typename BinOp, typename DerivedA, typename DerivedB>
-struct check_transpose_aliasing_run_time_selector<Scalar,DestIsTransposed,CwiseBinaryOp<BinOp,DerivedA,DerivedB> >
-{
-  static bool run(const Scalar* dest, const CwiseBinaryOp<BinOp,DerivedA,DerivedB>& src)
-  {
-    return ((blas_traits<DerivedA>::IsTransposed != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src.lhs())))
-        || ((blas_traits<DerivedB>::IsTransposed != DestIsTransposed) && (dest!=0 && dest==(const Scalar*)extract_data(src.rhs())));
-  }
-};
-
-// the following selector, checkTransposeAliasing_impl, based on MightHaveTransposeAliasing,
-// is because when the condition controlling the assert is known at compile time, ICC emits a warning.
-// This is actually a good warning: in expressions that don't have any transposing, the condition is
-// known at compile time to be false, and using that, we can avoid generating the code of the assert again
-// and again for all these expressions that don't need it.
-
-template<typename Derived, typename OtherDerived,
-         bool MightHaveTransposeAliasing
-                 = check_transpose_aliasing_compile_time_selector
-                     <blas_traits<Derived>::IsTransposed,OtherDerived>::ret
-        >
-struct checkTransposeAliasing_impl
-{
-    static void run(const Derived& dst, const OtherDerived& other)
-    {
-        eigen_assert((!check_transpose_aliasing_run_time_selector
-                      <typename Derived::Scalar,blas_traits<Derived>::IsTransposed,OtherDerived>
-                      ::run(extract_data(dst), other))
-          && "aliasing detected during transposition, use transposeInPlace() "
-             "or evaluate the rhs into a temporary using .eval()");
-
-    }
-};
-
-template<typename Derived, typename OtherDerived>
-struct checkTransposeAliasing_impl<Derived, OtherDerived, false>
-{
-    static void run(const Derived&, const OtherDerived&)
-    {
-    }
-};
-
-template<typename Dst, typename Src>
-void check_for_aliasing(const Dst &dst, const Src &src)
-{
-  internal::checkTransposeAliasing_impl<Dst, Src>::run(dst, src);
-}
-
-} // end namespace internal
-
-#endif // EIGEN_NO_DEBUG
-
-} // end namespace Eigen
-
-#endif // EIGEN_TRANSPOSE_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
deleted file mode 100644
index 667ef09..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
+++ /dev/null
@@ -1,983 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_TRIANGULARMATRIX_H
-#define EIGEN_TRIANGULARMATRIX_H
-
-namespace Eigen { 
-
-namespace internal {
-  
-template<int Side, typename TriangularType, typename Rhs> struct triangular_solve_retval;
-  
-}
-
-/** \class TriangularBase
-  * \ingroup Core_Module
-  *
-  * \brief Base class for triangular part in a matrix
-  */
-template<typename Derived> class TriangularBase : public EigenBase<Derived>
-{
-  public:
-
-    enum {
-      Mode = internal::traits<Derived>::Mode,
-      RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
-      ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
-      MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
-      MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
-      
-      SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
-                                                   internal::traits<Derived>::ColsAtCompileTime>::ret),
-      /**< This is equal to the number of coefficients, i.e. the number of
-          * rows times the number of columns, or to \a Dynamic if this is not
-          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
-      
-      MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
-                                                   internal::traits<Derived>::MaxColsAtCompileTime>::ret)
-        
-    };
-    typedef typename internal::traits<Derived>::Scalar Scalar;
-    typedef typename internal::traits<Derived>::StorageKind StorageKind;
-    typedef typename internal::traits<Derived>::StorageIndex StorageIndex;
-    typedef typename internal::traits<Derived>::FullMatrixType DenseMatrixType;
-    typedef DenseMatrixType DenseType;
-    typedef Derived const& Nested;
-
-    EIGEN_DEVICE_FUNC
-    inline TriangularBase() { eigen_assert(!((Mode&UnitDiag) && (Mode&ZeroDiag))); }
-
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return derived().rows(); }
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return derived().cols(); }
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const { return derived().outerStride(); }
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const { return derived().innerStride(); }
-    
-    // dummy resize function
-    void resize(Index rows, Index cols)
-    {
-      EIGEN_UNUSED_VARIABLE(rows);
-      EIGEN_UNUSED_VARIABLE(cols);
-      eigen_assert(rows==this->rows() && cols==this->cols());
-    }
-
-    EIGEN_DEVICE_FUNC
-    inline Scalar coeff(Index row, Index col) const  { return derived().coeff(row,col); }
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index row, Index col) { return derived().coeffRef(row,col); }
-
-    /** \see MatrixBase::copyCoeff(row,col)
-      */
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void copyCoeff(Index row, Index col, Other& other)
-    {
-      derived().coeffRef(row, col) = other.coeff(row, col);
-    }
-
-    EIGEN_DEVICE_FUNC
-    inline Scalar operator()(Index row, Index col) const
-    {
-      check_coordinates(row, col);
-      return coeff(row,col);
-    }
-    EIGEN_DEVICE_FUNC
-    inline Scalar& operator()(Index row, Index col)
-    {
-      check_coordinates(row, col);
-      return coeffRef(row,col);
-    }
-
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    EIGEN_DEVICE_FUNC
-    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
-    EIGEN_DEVICE_FUNC
-    inline Derived& derived() { return *static_cast<Derived*>(this); }
-    #endif // not EIGEN_PARSED_BY_DOXYGEN
-
-    template<typename DenseDerived>
-    EIGEN_DEVICE_FUNC
-    void evalTo(MatrixBase<DenseDerived> &other) const;
-    template<typename DenseDerived>
-    EIGEN_DEVICE_FUNC
-    void evalToLazy(MatrixBase<DenseDerived> &other) const;
-
-    EIGEN_DEVICE_FUNC
-    DenseMatrixType toDenseMatrix() const
-    {
-      DenseMatrixType res(rows(), cols());
-      evalToLazy(res);
-      return res;
-    }
-
-  protected:
-
-    void check_coordinates(Index row, Index col) const
-    {
-      EIGEN_ONLY_USED_FOR_DEBUG(row);
-      EIGEN_ONLY_USED_FOR_DEBUG(col);
-      eigen_assert(col>=0 && col<cols() && row>=0 && row<rows());
-      const int mode = int(Mode) & ~SelfAdjoint;
-      EIGEN_ONLY_USED_FOR_DEBUG(mode);
-      eigen_assert((mode==Upper && col>=row)
-                || (mode==Lower && col<=row)
-                || ((mode==StrictlyUpper || mode==UnitUpper) && col>row)
-                || ((mode==StrictlyLower || mode==UnitLower) && col<row));
-    }
-
-    #ifdef EIGEN_INTERNAL_DEBUGGING
-    void check_coordinates_internal(Index row, Index col) const
-    {
-      check_coordinates(row, col);
-    }
-    #else
-    void check_coordinates_internal(Index , Index ) const {}
-    #endif
-
-};
-
-/** \class TriangularView
-  * \ingroup Core_Module
-  *
-  * \brief Expression of a triangular part in a matrix
-  *
-  * \param MatrixType the type of the object in which we are taking the triangular part
-  * \param Mode the kind of triangular matrix expression to construct. Can be #Upper,
-  *             #Lower, #UnitUpper, #UnitLower, #StrictlyUpper, or #StrictlyLower.
-  *             This is in fact a bit field; it must have either #Upper or #Lower, 
-  *             and additionally it may have #UnitDiag or #ZeroDiag or neither.
-  *
-  * This class represents a triangular part of a matrix, not necessarily square. Strictly speaking, for rectangular
-  * matrices one should speak of "trapezoid" parts. This class is the return type
-  * of MatrixBase::triangularView() and SparseMatrixBase::triangularView(), and most of the time this is the only way it is used.
-  *
-  * \sa MatrixBase::triangularView()
-  */
-namespace internal {
-template<typename MatrixType, unsigned int _Mode>
-struct traits<TriangularView<MatrixType, _Mode> > : traits<MatrixType>
-{
-  typedef typename ref_selector<MatrixType>::non_const_type MatrixTypeNested;
-  typedef typename remove_reference<MatrixTypeNested>::type MatrixTypeNestedNonRef;
-  typedef typename remove_all<MatrixTypeNested>::type MatrixTypeNestedCleaned;
-  typedef typename MatrixType::PlainObject FullMatrixType;
-  typedef MatrixType ExpressionType;
-  enum {
-    Mode = _Mode,
-    FlagsLvalueBit = is_lvalue<MatrixType>::value ? LvalueBit : 0,
-    Flags = (MatrixTypeNestedCleaned::Flags & (HereditaryBits | FlagsLvalueBit) & (~(PacketAccessBit | DirectAccessBit | LinearAccessBit)))
-  };
-};
-}
-
-template<typename _MatrixType, unsigned int _Mode, typename StorageKind> class TriangularViewImpl;
-
-template<typename _MatrixType, unsigned int _Mode> class TriangularView
-  : public TriangularViewImpl<_MatrixType, _Mode, typename internal::traits<_MatrixType>::StorageKind >
-{
-  public:
-
-    typedef TriangularViewImpl<_MatrixType, _Mode, typename internal::traits<_MatrixType>::StorageKind > Base;
-    typedef typename internal::traits<TriangularView>::Scalar Scalar;
-    typedef _MatrixType MatrixType;
-
-  protected:
-    typedef typename internal::traits<TriangularView>::MatrixTypeNested MatrixTypeNested;
-    typedef typename internal::traits<TriangularView>::MatrixTypeNestedNonRef MatrixTypeNestedNonRef;
-
-    typedef typename internal::remove_all<typename MatrixType::ConjugateReturnType>::type MatrixConjugateReturnType;
-    
-  public:
-
-    typedef typename internal::traits<TriangularView>::StorageKind StorageKind;
-    typedef typename internal::traits<TriangularView>::MatrixTypeNestedCleaned NestedExpression;
-
-    enum {
-      Mode = _Mode,
-      Flags = internal::traits<TriangularView>::Flags,
-      TransposeMode = (Mode & Upper ? Lower : 0)
-                    | (Mode & Lower ? Upper : 0)
-                    | (Mode & (UnitDiag))
-                    | (Mode & (ZeroDiag)),
-      IsVectorAtCompileTime = false
-    };
-
-    EIGEN_DEVICE_FUNC
-    explicit inline TriangularView(MatrixType& matrix) : m_matrix(matrix)
-    {}
-    
-    using Base::operator=;
-    TriangularView& operator=(const TriangularView &other)
-    { return Base::operator=(other); }
-
-    /** \copydoc EigenBase::rows() */
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return m_matrix.rows(); }
-    /** \copydoc EigenBase::cols() */
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return m_matrix.cols(); }
-
-    /** \returns a const reference to the nested expression */
-    EIGEN_DEVICE_FUNC
-    const NestedExpression& nestedExpression() const { return m_matrix; }
-
-    /** \returns a reference to the nested expression */
-    EIGEN_DEVICE_FUNC
-    NestedExpression& nestedExpression() { return m_matrix; }
-    
-    typedef TriangularView<const MatrixConjugateReturnType,Mode> ConjugateReturnType;
-    /** \sa MatrixBase::conjugate() const */
-    EIGEN_DEVICE_FUNC
-    inline const ConjugateReturnType conjugate() const
-    { return ConjugateReturnType(m_matrix.conjugate()); }
-
-    typedef TriangularView<const typename MatrixType::AdjointReturnType,TransposeMode> AdjointReturnType;
-    /** \sa MatrixBase::adjoint() const */
-    EIGEN_DEVICE_FUNC
-    inline const AdjointReturnType adjoint() const
-    { return AdjointReturnType(m_matrix.adjoint()); }
-
-    typedef TriangularView<typename MatrixType::TransposeReturnType,TransposeMode> TransposeReturnType;
-     /** \sa MatrixBase::transpose() */
-    EIGEN_DEVICE_FUNC
-    inline TransposeReturnType transpose()
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(MatrixType)
-      typename MatrixType::TransposeReturnType tmp(m_matrix);
-      return TransposeReturnType(tmp);
-    }
-    
-    typedef TriangularView<const typename MatrixType::ConstTransposeReturnType,TransposeMode> ConstTransposeReturnType;
-    /** \sa MatrixBase::transpose() const */
-    EIGEN_DEVICE_FUNC
-    inline const ConstTransposeReturnType transpose() const
-    {
-      return ConstTransposeReturnType(m_matrix.transpose());
-    }
-
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    inline const Solve<TriangularView, Other> 
-    solve(const MatrixBase<Other>& other) const
-    { return Solve<TriangularView, Other>(*this, other.derived()); }
-    
-  // workaround MSVC ICE
-  #if EIGEN_COMP_MSVC
-    template<int Side, typename Other>
-    EIGEN_DEVICE_FUNC
-    inline const internal::triangular_solve_retval<Side,TriangularView, Other>
-    solve(const MatrixBase<Other>& other) const
-    { return Base::template solve<Side>(other); }
-  #else
-    using Base::solve;
-  #endif
-
-    /** \returns a selfadjoint view of the referenced triangular part which must be either \c #Upper or \c #Lower.
-      *
-      * This is a shortcut for \code this->nestedExpression().selfadjointView<(*this)::Mode>() \endcode
-      * \sa MatrixBase::selfadjointView() */
-    EIGEN_DEVICE_FUNC
-    SelfAdjointView<MatrixTypeNestedNonRef,Mode> selfadjointView()
-    {
-      EIGEN_STATIC_ASSERT((Mode&(UnitDiag|ZeroDiag))==0,PROGRAMMING_ERROR);
-      return SelfAdjointView<MatrixTypeNestedNonRef,Mode>(m_matrix);
-    }
-
-    /** This is the const version of selfadjointView() */
-    EIGEN_DEVICE_FUNC
-    const SelfAdjointView<MatrixTypeNestedNonRef,Mode> selfadjointView() const
-    {
-      EIGEN_STATIC_ASSERT((Mode&(UnitDiag|ZeroDiag))==0,PROGRAMMING_ERROR);
-      return SelfAdjointView<MatrixTypeNestedNonRef,Mode>(m_matrix);
-    }
-
-
-    /** \returns the determinant of the triangular matrix
-      * \sa MatrixBase::determinant() */
-    EIGEN_DEVICE_FUNC
-    Scalar determinant() const
-    {
-      if (Mode & UnitDiag)
-        return 1;
-      else if (Mode & ZeroDiag)
-        return 0;
-      else
-        return m_matrix.diagonal().prod();
-    }
-      
-  protected:
-
-    MatrixTypeNested m_matrix;
-};
-
-/** \ingroup Core_Module
-  *
-  * \brief Base class for a triangular part in a \b dense matrix
-  *
-  * This class is an abstract base class of class TriangularView, and objects of type TriangularViewImpl cannot be instantiated.
-  * It extends class TriangularView with additional methods which available for dense expressions only.
-  *
-  * \sa class TriangularView, MatrixBase::triangularView()
-  */
-template<typename _MatrixType, unsigned int _Mode> class TriangularViewImpl<_MatrixType,_Mode,Dense>
-  : public TriangularBase<TriangularView<_MatrixType, _Mode> >
-{
-  public:
-
-    typedef TriangularView<_MatrixType, _Mode> TriangularViewType;
-    typedef TriangularBase<TriangularViewType> Base;
-    typedef typename internal::traits<TriangularViewType>::Scalar Scalar;
-
-    typedef _MatrixType MatrixType;
-    typedef typename MatrixType::PlainObject DenseMatrixType;
-    typedef DenseMatrixType PlainObject;
-
-  public:
-    using Base::evalToLazy;
-    using Base::derived;
-
-    typedef typename internal::traits<TriangularViewType>::StorageKind StorageKind;
-
-    enum {
-      Mode = _Mode,
-      Flags = internal::traits<TriangularViewType>::Flags
-    };
-
-    /** \returns the outer-stride of the underlying dense matrix
-      * \sa DenseCoeffsBase::outerStride() */
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const { return derived().nestedExpression().outerStride(); }
-    /** \returns the inner-stride of the underlying dense matrix
-      * \sa DenseCoeffsBase::innerStride() */
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const { return derived().nestedExpression().innerStride(); }
-
-    /** \sa MatrixBase::operator+=() */
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator+=(const DenseBase<Other>& other) {
-      internal::call_assignment_no_alias(derived(), other.derived(), internal::add_assign_op<Scalar,typename Other::Scalar>());
-      return derived();
-    }
-    /** \sa MatrixBase::operator-=() */
-    template<typename Other>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator-=(const DenseBase<Other>& other) {
-      internal::call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op<Scalar,typename Other::Scalar>());
-      return derived();
-    }
-    
-    /** \sa MatrixBase::operator*=() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator*=(const typename internal::traits<MatrixType>::Scalar& other) { return *this = derived().nestedExpression() * other; }
-    /** \sa DenseBase::operator/=() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType&  operator/=(const typename internal::traits<MatrixType>::Scalar& other) { return *this = derived().nestedExpression() / other; }
-
-    /** \sa MatrixBase::fill() */
-    EIGEN_DEVICE_FUNC
-    void fill(const Scalar& value) { setConstant(value); }
-    /** \sa MatrixBase::setConstant() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& setConstant(const Scalar& value)
-    { return *this = MatrixType::Constant(derived().rows(), derived().cols(), value); }
-    /** \sa MatrixBase::setZero() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& setZero() { return setConstant(Scalar(0)); }
-    /** \sa MatrixBase::setOnes() */
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& setOnes() { return setConstant(Scalar(1)); }
-
-    /** \sa MatrixBase::coeff()
-      * \warning the coordinates must fit into the referenced triangular part
-      */
-    EIGEN_DEVICE_FUNC
-    inline Scalar coeff(Index row, Index col) const
-    {
-      Base::check_coordinates_internal(row, col);
-      return derived().nestedExpression().coeff(row, col);
-    }
-
-    /** \sa MatrixBase::coeffRef()
-      * \warning the coordinates must fit into the referenced triangular part
-      */
-    EIGEN_DEVICE_FUNC
-    inline Scalar& coeffRef(Index row, Index col)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(TriangularViewType);
-      Base::check_coordinates_internal(row, col);
-      return derived().nestedExpression().coeffRef(row, col);
-    }
-
-    /** Assigns a triangular matrix to a triangular part of a dense matrix */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& operator=(const TriangularBase<OtherDerived>& other);
-
-    /** Shortcut for\code *this = other.other.triangularView<(*this)::Mode>() \endcode */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& operator=(const MatrixBase<OtherDerived>& other);
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    EIGEN_DEVICE_FUNC
-    TriangularViewType& operator=(const TriangularViewImpl& other)
-    { return *this = other.derived().nestedExpression(); }
-
-    /** \deprecated */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void lazyAssign(const TriangularBase<OtherDerived>& other);
-
-    /** \deprecated */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void lazyAssign(const MatrixBase<OtherDerived>& other);
-#endif
-
-    /** Efficient triangular matrix times vector/matrix product */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    const Product<TriangularViewType,OtherDerived>
-    operator*(const MatrixBase<OtherDerived>& rhs) const
-    {
-      return Product<TriangularViewType,OtherDerived>(derived(), rhs.derived());
-    }
-
-    /** Efficient vector/matrix times triangular matrix product */
-    template<typename OtherDerived> friend
-    EIGEN_DEVICE_FUNC
-    const Product<OtherDerived,TriangularViewType>
-    operator*(const MatrixBase<OtherDerived>& lhs, const TriangularViewImpl& rhs)
-    {
-      return Product<OtherDerived,TriangularViewType>(lhs.derived(),rhs.derived());
-    }
-
-    /** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
-      *
-      * This function computes the inverse-matrix matrix product inverse(\c *this) * \a other if
-      * \a Side==OnTheLeft (the default), or the right-inverse-multiply  \a other * inverse(\c *this) if
-      * \a Side==OnTheRight.
-      *
-      * Note that the template parameter \c Side can be ommitted, in which case \c Side==OnTheLeft
-      *
-      * The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
-      * diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
-      * is an upper (resp. lower) triangular matrix.
-      *
-      * Example: \include Triangular_solve.cpp
-      * Output: \verbinclude Triangular_solve.out
-      *
-      * This function returns an expression of the inverse-multiply and can works in-place if it is assigned
-      * to the same matrix or vector \a other.
-      *
-      * For users coming from BLAS, this function (and more specifically solveInPlace()) offer
-      * all the operations supported by the \c *TRSV and \c *TRSM BLAS routines.
-      *
-      * \sa TriangularView::solveInPlace()
-      */
-    template<int Side, typename Other>
-    EIGEN_DEVICE_FUNC
-    inline const internal::triangular_solve_retval<Side,TriangularViewType, Other>
-    solve(const MatrixBase<Other>& other) const;
-
-    /** "in-place" version of TriangularView::solve() where the result is written in \a other
-      *
-      * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
-      * This function will const_cast it, so constness isn't honored here.
-      *
-      * Note that the template parameter \c Side can be ommitted, in which case \c Side==OnTheLeft
-      *
-      * See TriangularView:solve() for the details.
-      */
-    template<int Side, typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void solveInPlace(const MatrixBase<OtherDerived>& other) const;
-
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void solveInPlace(const MatrixBase<OtherDerived>& other) const
-    { return solveInPlace<OnTheLeft>(other); }
-
-    /** Swaps the coefficients of the common triangular parts of two matrices */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-#ifdef EIGEN_PARSED_BY_DOXYGEN
-    void swap(TriangularBase<OtherDerived> &other)
-#else
-    void swap(TriangularBase<OtherDerived> const & other)
-#endif
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
-      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
-    }
-
-    /** \deprecated
-      * Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */
-    template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
-    void swap(MatrixBase<OtherDerived> const & other)
-    {
-      EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
-      call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
-    }
-
-    template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE void _solve_impl(const RhsType &rhs, DstType &dst) const {
-      if(!internal::is_same_dense(dst,rhs))
-        dst = rhs;
-      this->solveInPlace(dst);
-    }
-
-    template<typename ProductType>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE TriangularViewType& _assignProduct(const ProductType& prod, const Scalar& alpha, bool beta);
-};
-
-/***************************************************************************
-* Implementation of triangular evaluation/assignment
-***************************************************************************/
-
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-// FIXME should we keep that possibility
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-inline TriangularView<MatrixType, Mode>&
-TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const MatrixBase<OtherDerived>& other)
-{
-  internal::call_assignment_no_alias(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
-  return derived();
-}
-
-// FIXME should we keep that possibility
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const MatrixBase<OtherDerived>& other)
-{
-  internal::call_assignment_no_alias(derived(), other.template triangularView<Mode>());
-}
-
-
-
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-inline TriangularView<MatrixType, Mode>&
-TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const TriangularBase<OtherDerived>& other)
-{
-  eigen_assert(Mode == int(OtherDerived::Mode));
-  internal::call_assignment(derived(), other.derived());
-  return derived();
-}
-
-template<typename MatrixType, unsigned int Mode>
-template<typename OtherDerived>
-void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const TriangularBase<OtherDerived>& other)
-{
-  eigen_assert(Mode == int(OtherDerived::Mode));
-  internal::call_assignment_no_alias(derived(), other.derived());
-}
-#endif
-
-/***************************************************************************
-* Implementation of TriangularBase methods
-***************************************************************************/
-
-/** Assigns a triangular or selfadjoint matrix to a dense matrix.
-  * If the matrix is triangular, the opposite part is set to zero. */
-template<typename Derived>
-template<typename DenseDerived>
-void TriangularBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
-{
-  evalToLazy(other.derived());
-}
-
-/***************************************************************************
-* Implementation of TriangularView methods
-***************************************************************************/
-
-/***************************************************************************
-* Implementation of MatrixBase methods
-***************************************************************************/
-
-/**
-  * \returns an expression of a triangular view extracted from the current matrix
-  *
-  * The parameter \a Mode can have the following values: \c #Upper, \c #StrictlyUpper, \c #UnitUpper,
-  * \c #Lower, \c #StrictlyLower, \c #UnitLower.
-  *
-  * Example: \include MatrixBase_triangularView.cpp
-  * Output: \verbinclude MatrixBase_triangularView.out
-  *
-  * \sa class TriangularView
-  */
-template<typename Derived>
-template<unsigned int Mode>
-typename MatrixBase<Derived>::template TriangularViewReturnType<Mode>::Type
-MatrixBase<Derived>::triangularView()
-{
-  return typename TriangularViewReturnType<Mode>::Type(derived());
-}
-
-/** This is the const version of MatrixBase::triangularView() */
-template<typename Derived>
-template<unsigned int Mode>
-typename MatrixBase<Derived>::template ConstTriangularViewReturnType<Mode>::Type
-MatrixBase<Derived>::triangularView() const
-{
-  return typename ConstTriangularViewReturnType<Mode>::Type(derived());
-}
-
-/** \returns true if *this is approximately equal to an upper triangular matrix,
-  *          within the precision given by \a prec.
-  *
-  * \sa isLowerTriangular()
-  */
-template<typename Derived>
-bool MatrixBase<Derived>::isUpperTriangular(const RealScalar& prec) const
-{
-  RealScalar maxAbsOnUpperPart = static_cast<RealScalar>(-1);
-  for(Index j = 0; j < cols(); ++j)
-  {
-    Index maxi = numext::mini(j, rows()-1);
-    for(Index i = 0; i <= maxi; ++i)
-    {
-      RealScalar absValue = numext::abs(coeff(i,j));
-      if(absValue > maxAbsOnUpperPart) maxAbsOnUpperPart = absValue;
-    }
-  }
-  RealScalar threshold = maxAbsOnUpperPart * prec;
-  for(Index j = 0; j < cols(); ++j)
-    for(Index i = j+1; i < rows(); ++i)
-      if(numext::abs(coeff(i, j)) > threshold) return false;
-  return true;
-}
-
-/** \returns true if *this is approximately equal to a lower triangular matrix,
-  *          within the precision given by \a prec.
-  *
-  * \sa isUpperTriangular()
-  */
-template<typename Derived>
-bool MatrixBase<Derived>::isLowerTriangular(const RealScalar& prec) const
-{
-  RealScalar maxAbsOnLowerPart = static_cast<RealScalar>(-1);
-  for(Index j = 0; j < cols(); ++j)
-    for(Index i = j; i < rows(); ++i)
-    {
-      RealScalar absValue = numext::abs(coeff(i,j));
-      if(absValue > maxAbsOnLowerPart) maxAbsOnLowerPart = absValue;
-    }
-  RealScalar threshold = maxAbsOnLowerPart * prec;
-  for(Index j = 1; j < cols(); ++j)
-  {
-    Index maxi = numext::mini(j, rows()-1);
-    for(Index i = 0; i < maxi; ++i)
-      if(numext::abs(coeff(i, j)) > threshold) return false;
-  }
-  return true;
-}
-
-
-/***************************************************************************
-****************************************************************************
-* Evaluators and Assignment of triangular expressions
-***************************************************************************
-***************************************************************************/
-
-namespace internal {
-
-  
-// TODO currently a triangular expression has the form TriangularView<.,.>
-//      in the future triangular-ness should be defined by the expression traits
-//      such that Transpose<TriangularView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
-template<typename MatrixType, unsigned int Mode>
-struct evaluator_traits<TriangularView<MatrixType,Mode> >
-{
-  typedef typename storage_kind_to_evaluator_kind<typename MatrixType::StorageKind>::Kind Kind;
-  typedef typename glue_shapes<typename evaluator_traits<MatrixType>::Shape, TriangularShape>::type Shape;
-};
-
-template<typename MatrixType, unsigned int Mode>
-struct unary_evaluator<TriangularView<MatrixType,Mode>, IndexBased>
- : evaluator<typename internal::remove_all<MatrixType>::type>
-{
-  typedef TriangularView<MatrixType,Mode> XprType;
-  typedef evaluator<typename internal::remove_all<MatrixType>::type> Base;
-  unary_evaluator(const XprType &xpr) : Base(xpr.nestedExpression()) {}
-};
-
-// Additional assignment kinds:
-struct Triangular2Triangular    {};
-struct Triangular2Dense         {};
-struct Dense2Triangular         {};
-
-
-template<typename Kernel, unsigned int Mode, int UnrollCount, bool ClearOpposite> struct triangular_assignment_loop;
-
- 
-/** \internal Specialization of the dense assignment kernel for triangular matrices.
-  * The main difference is that the triangular, diagonal, and opposite parts are processed through three different functions.
-  * \tparam UpLo must be either Lower or Upper
-  * \tparam Mode must be either 0, UnitDiag, ZeroDiag, or SelfAdjoint
-  */
-template<int UpLo, int Mode, int SetOpposite, typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor, int Version = Specialized>
-class triangular_dense_assignment_kernel : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version>
-{
-protected:
-  typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version> Base;
-  typedef typename Base::DstXprType DstXprType;
-  typedef typename Base::SrcXprType SrcXprType;
-  using Base::m_dst;
-  using Base::m_src;
-  using Base::m_functor;
-public:
-  
-  typedef typename Base::DstEvaluatorType DstEvaluatorType;
-  typedef typename Base::SrcEvaluatorType SrcEvaluatorType;
-  typedef typename Base::Scalar Scalar;
-  typedef typename Base::AssignmentTraits AssignmentTraits;
-  
-  
-  EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
-    : Base(dst, src, func, dstExpr)
-  {}
-  
-#ifdef EIGEN_INTERNAL_DEBUGGING
-  EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col)
-  {
-    eigen_internal_assert(row!=col);
-    Base::assignCoeff(row,col);
-  }
-#else
-  using Base::assignCoeff;
-#endif
-  
-  EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id)
-  {
-         if(Mode==UnitDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(1));
-    else if(Mode==ZeroDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(0));
-    else if(Mode==0)                       Base::assignCoeff(id,id);
-  }
-  
-  EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index row, Index col)
-  { 
-    eigen_internal_assert(row!=col);
-    if(SetOpposite)
-      m_functor.assignCoeff(m_dst.coeffRef(row,col), Scalar(0));
-  }
-};
-
-template<int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType, typename Functor>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src, const Functor &func)
-{
-  typedef evaluator<DstXprType> DstEvaluatorType;
-  typedef evaluator<SrcXprType> SrcEvaluatorType;
-
-  SrcEvaluatorType srcEvaluator(src);
-
-  Index dstRows = src.rows();
-  Index dstCols = src.cols();
-  if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-    dst.resize(dstRows, dstCols);
-  DstEvaluatorType dstEvaluator(dst);
-    
-  typedef triangular_dense_assignment_kernel< Mode&(Lower|Upper),Mode&(UnitDiag|ZeroDiag|SelfAdjoint),SetOpposite,
-                                              DstEvaluatorType,SrcEvaluatorType,Functor> Kernel;
-  Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
-  
-  enum {
-      unroll = DstXprType::SizeAtCompileTime != Dynamic
-            && SrcEvaluatorType::CoeffReadCost < HugeCost
-            && DstXprType::SizeAtCompileTime * (DstEvaluatorType::CoeffReadCost+SrcEvaluatorType::CoeffReadCost) / 2 <= EIGEN_UNROLLING_LIMIT
-    };
-  
-  triangular_assignment_loop<Kernel, Mode, unroll ? int(DstXprType::SizeAtCompileTime) : Dynamic, SetOpposite>::run(kernel);
-}
-
-template<int Mode, bool SetOpposite, typename DstXprType, typename SrcXprType>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-void call_triangular_assignment_loop(DstXprType& dst, const SrcXprType& src)
-{
-  call_triangular_assignment_loop<Mode,SetOpposite>(dst, src, internal::assign_op<typename DstXprType::Scalar,typename SrcXprType::Scalar>());
-}
-
-template<> struct AssignmentKind<TriangularShape,TriangularShape> { typedef Triangular2Triangular Kind; };
-template<> struct AssignmentKind<DenseShape,TriangularShape>      { typedef Triangular2Dense      Kind; };
-template<> struct AssignmentKind<TriangularShape,DenseShape>      { typedef Dense2Triangular      Kind; };
-
-
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Triangular>
-{
-  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
-    eigen_assert(int(DstXprType::Mode) == int(SrcXprType::Mode));
-    
-    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);  
-  }
-};
-
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Triangular2Dense>
-{
-  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
-    call_triangular_assignment_loop<SrcXprType::Mode, (SrcXprType::Mode&SelfAdjoint)==0>(dst, src, func);  
-  }
-};
-
-template< typename DstXprType, typename SrcXprType, typename Functor>
-struct Assignment<DstXprType, SrcXprType, Functor, Dense2Triangular>
-{
-  EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
-  {
-    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);  
-  }
-};
-
-
-template<typename Kernel, unsigned int Mode, int UnrollCount, bool SetOpposite>
-struct triangular_assignment_loop
-{
-  // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
-  typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
-  typedef typename DstEvaluatorType::XprType DstXprType;
-  
-  enum {
-    col = (UnrollCount-1) / DstXprType::RowsAtCompileTime,
-    row = (UnrollCount-1) % DstXprType::RowsAtCompileTime
-  };
-  
-  typedef typename Kernel::Scalar Scalar;
-
-  EIGEN_DEVICE_FUNC
-  static inline void run(Kernel &kernel)
-  {
-    triangular_assignment_loop<Kernel, Mode, UnrollCount-1, SetOpposite>::run(kernel);
-    
-    if(row==col)
-      kernel.assignDiagonalCoeff(row);
-    else if( ((Mode&Lower) && row>col) || ((Mode&Upper) && row<col) )
-      kernel.assignCoeff(row,col);
-    else if(SetOpposite)
-      kernel.assignOppositeCoeff(row,col);
-  }
-};
-
-// prevent buggy user code from causing an infinite recursion
-template<typename Kernel, unsigned int Mode, bool SetOpposite>
-struct triangular_assignment_loop<Kernel, Mode, 0, SetOpposite>
-{
-  EIGEN_DEVICE_FUNC
-  static inline void run(Kernel &) {}
-};
-
-
-
-// TODO: experiment with a recursive assignment procedure splitting the current
-//       triangular part into one rectangular and two triangular parts.
-
-
-template<typename Kernel, unsigned int Mode, bool SetOpposite>
-struct triangular_assignment_loop<Kernel, Mode, Dynamic, SetOpposite>
-{
-  typedef typename Kernel::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  static inline void run(Kernel &kernel)
-  {
-    for(Index j = 0; j < kernel.cols(); ++j)
-    {
-      Index maxi = numext::mini(j, kernel.rows());
-      Index i = 0;
-      if (((Mode&Lower) && SetOpposite) || (Mode&Upper))
-      {
-        for(; i < maxi; ++i)
-          if(Mode&Upper) kernel.assignCoeff(i, j);
-          else           kernel.assignOppositeCoeff(i, j);
-      }
-      else
-        i = maxi;
-      
-      if(i<kernel.rows()) // then i==j
-        kernel.assignDiagonalCoeff(i++);
-      
-      if (((Mode&Upper) && SetOpposite) || (Mode&Lower))
-      {
-        for(; i < kernel.rows(); ++i)
-          if(Mode&Lower) kernel.assignCoeff(i, j);
-          else           kernel.assignOppositeCoeff(i, j);
-      }
-    }
-  }
-};
-
-} // end namespace internal
-
-/** Assigns a triangular or selfadjoint matrix to a dense matrix.
-  * If the matrix is triangular, the opposite part is set to zero. */
-template<typename Derived>
-template<typename DenseDerived>
-void TriangularBase<Derived>::evalToLazy(MatrixBase<DenseDerived> &other) const
-{
-  other.derived().resize(this->rows(), this->cols());
-  internal::call_triangular_assignment_loop<Derived::Mode,(Derived::Mode&SelfAdjoint)==0 /* SetOpposite */>(other.derived(), derived().nestedExpression());
-}
-
-namespace internal {
-  
-// Triangular = Product
-template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
-{
-  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,typename SrcXprType::Scalar> &)
-  {
-    Index dstRows = src.rows();
-    Index dstCols = src.cols();
-    if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
-      dst.resize(dstRows, dstCols);
-
-    dst._assignProduct(src, 1, 0);
-  }
-};
-
-// Triangular += Product
-template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::add_assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
-{
-  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<Scalar,typename SrcXprType::Scalar> &)
-  {
-    dst._assignProduct(src, 1, 1);
-  }
-};
-
-// Triangular -= Product
-template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
-struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::sub_assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
-{
-  typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
-  static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<Scalar,typename SrcXprType::Scalar> &)
-  {
-    dst._assignProduct(src, -1, 1);
-  }
-};
-
-} // end namespace internal
-
-} // end namespace Eigen
-
-#endif // EIGEN_TRIANGULARMATRIX_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
deleted file mode 100644
index aa054a0..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
+++ /dev/null
@@ -1,1001 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MACROS_H
-#define EIGEN_MACROS_H
-
-#define EIGEN_WORLD_VERSION 3
-#define EIGEN_MAJOR_VERSION 3
-#define EIGEN_MINOR_VERSION 7
-
-#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
-                                      (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
-                                                                 EIGEN_MINOR_VERSION>=z))))
-
-// Compiler identification, EIGEN_COMP_*
-
-/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
-#ifdef __GNUC__
-  #define EIGEN_COMP_GNUC 1
-#else
-  #define EIGEN_COMP_GNUC 0
-#endif
-
-/// \internal EIGEN_COMP_CLANG set to major+minor version (e.g., 307 for clang 3.7) if the compiler is clang
-#if defined(__clang__)
-  #define EIGEN_COMP_CLANG (__clang_major__*100+__clang_minor__)
-#else
-  #define EIGEN_COMP_CLANG 0
-#endif
-
-
-/// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm
-#if defined(__llvm__)
-  #define EIGEN_COMP_LLVM 1
-#else
-  #define EIGEN_COMP_LLVM 0
-#endif
-
-/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel compiler, 0 otherwise
-#if defined(__INTEL_COMPILER)
-  #define EIGEN_COMP_ICC __INTEL_COMPILER
-#else
-  #define EIGEN_COMP_ICC 0
-#endif
-
-/// \internal EIGEN_COMP_MINGW set to 1 if the compiler is mingw
-#if defined(__MINGW32__)
-  #define EIGEN_COMP_MINGW 1
-#else
-  #define EIGEN_COMP_MINGW 0
-#endif
-
-/// \internal EIGEN_COMP_SUNCC set to 1 if the compiler is Solaris Studio
-#if defined(__SUNPRO_CC)
-  #define EIGEN_COMP_SUNCC 1
-#else
-  #define EIGEN_COMP_SUNCC 0
-#endif
-
-/// \internal EIGEN_COMP_MSVC set to _MSC_VER if the compiler is Microsoft Visual C++, 0 otherwise.
-#if defined(_MSC_VER)
-  #define EIGEN_COMP_MSVC _MSC_VER
-#else
-  #define EIGEN_COMP_MSVC 0
-#endif
-
-// For the record, here is a table summarizing the possible values for EIGEN_COMP_MSVC:
-//  name  ver   MSC_VER
-//  2008    9      1500
-//  2010   10      1600
-//  2012   11      1700
-//  2013   12      1800
-//  2015   14      1900
-//  "15"   15      1900
-
-/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC or clang-cl
-#if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC || EIGEN_COMP_LLVM || EIGEN_COMP_CLANG)
-  #define EIGEN_COMP_MSVC_STRICT _MSC_VER
-#else
-  #define EIGEN_COMP_MSVC_STRICT 0
-#endif
-
-/// \internal EIGEN_COMP_IBM set to 1 if the compiler is IBM XL C++
-#if defined(__IBMCPP__) || defined(__xlc__)
-  #define EIGEN_COMP_IBM 1
-#else
-  #define EIGEN_COMP_IBM 0
-#endif
-
-/// \internal EIGEN_COMP_PGI set to 1 if the compiler is Portland Group Compiler
-#if defined(__PGI)
-  #define EIGEN_COMP_PGI 1
-#else
-  #define EIGEN_COMP_PGI 0
-#endif
-
-/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION)
-  #define EIGEN_COMP_ARM 1
-#else
-  #define EIGEN_COMP_ARM 0
-#endif
-
-/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
-#if defined(__EMSCRIPTEN__)
-  #define EIGEN_COMP_EMSCRIPTEN 1
-#else
-  #define EIGEN_COMP_EMSCRIPTEN 0
-#endif
-
-
-/// \internal EIGEN_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC, clang, mingw, etc.)
-#if EIGEN_COMP_GNUC && !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM || EIGEN_COMP_EMSCRIPTEN)
-  #define EIGEN_COMP_GNUC_STRICT 1
-#else
-  #define EIGEN_COMP_GNUC_STRICT 0
-#endif
-
-
-#if EIGEN_COMP_GNUC
-  #define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__==x && __GNUC_MINOR__>=y) || __GNUC__>x)
-  #define EIGEN_GNUC_AT_MOST(x,y)  ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
-  #define EIGEN_GNUC_AT(x,y)       ( __GNUC__==x && __GNUC_MINOR__==y )
-#else
-  #define EIGEN_GNUC_AT_LEAST(x,y) 0
-  #define EIGEN_GNUC_AT_MOST(x,y)  0
-  #define EIGEN_GNUC_AT(x,y)       0
-#endif
-
-// FIXME: could probably be removed as we do not support gcc 3.x anymore
-#if EIGEN_COMP_GNUC && (__GNUC__ <= 3)
-#define EIGEN_GCC3_OR_OLDER 1
-#else
-#define EIGEN_GCC3_OR_OLDER 0
-#endif
-
-
-// Architecture identification, EIGEN_ARCH_*
-
-#if defined(__x86_64__) || defined(_M_X64) || defined(__amd64)
-  #define EIGEN_ARCH_x86_64 1
-#else
-  #define EIGEN_ARCH_x86_64 0
-#endif
-
-#if defined(__i386__) || defined(_M_IX86) || defined(_X86_) || defined(__i386)
-  #define EIGEN_ARCH_i386 1
-#else
-  #define EIGEN_ARCH_i386 0
-#endif
-
-#if EIGEN_ARCH_x86_64 || EIGEN_ARCH_i386
-  #define EIGEN_ARCH_i386_OR_x86_64 1
-#else
-  #define EIGEN_ARCH_i386_OR_x86_64 0
-#endif
-
-/// \internal EIGEN_ARCH_ARM set to 1 if the architecture is ARM
-#if defined(__arm__)
-  #define EIGEN_ARCH_ARM 1
-#else
-  #define EIGEN_ARCH_ARM 0
-#endif
-
-/// \internal EIGEN_ARCH_ARM64 set to 1 if the architecture is ARM64
-#if defined(__aarch64__)
-  #define EIGEN_ARCH_ARM64 1
-#else
-  #define EIGEN_ARCH_ARM64 0
-#endif
-
-#if EIGEN_ARCH_ARM || EIGEN_ARCH_ARM64
-  #define EIGEN_ARCH_ARM_OR_ARM64 1
-#else
-  #define EIGEN_ARCH_ARM_OR_ARM64 0
-#endif
-
-/// \internal EIGEN_ARCH_MIPS set to 1 if the architecture is MIPS
-#if defined(__mips__) || defined(__mips)
-  #define EIGEN_ARCH_MIPS 1
-#else
-  #define EIGEN_ARCH_MIPS 0
-#endif
-
-/// \internal EIGEN_ARCH_SPARC set to 1 if the architecture is SPARC
-#if defined(__sparc__) || defined(__sparc)
-  #define EIGEN_ARCH_SPARC 1
-#else
-  #define EIGEN_ARCH_SPARC 0
-#endif
-
-/// \internal EIGEN_ARCH_IA64 set to 1 if the architecture is Intel Itanium
-#if defined(__ia64__)
-  #define EIGEN_ARCH_IA64 1
-#else
-  #define EIGEN_ARCH_IA64 0
-#endif
-
-/// \internal EIGEN_ARCH_PPC set to 1 if the architecture is PowerPC
-#if defined(__powerpc__) || defined(__ppc__) || defined(_M_PPC)
-  #define EIGEN_ARCH_PPC 1
-#else
-  #define EIGEN_ARCH_PPC 0
-#endif
-
-
-
-// Operating system identification, EIGEN_OS_*
-
-/// \internal EIGEN_OS_UNIX set to 1 if the OS is a unix variant
-#if defined(__unix__) || defined(__unix)
-  #define EIGEN_OS_UNIX 1
-#else
-  #define EIGEN_OS_UNIX 0
-#endif
-
-/// \internal EIGEN_OS_LINUX set to 1 if the OS is based on Linux kernel
-#if defined(__linux__)
-  #define EIGEN_OS_LINUX 1
-#else
-  #define EIGEN_OS_LINUX 0
-#endif
-
-/// \internal EIGEN_OS_ANDROID set to 1 if the OS is Android
-// note: ANDROID is defined when using ndk_build, __ANDROID__ is defined when using a standalone toolchain.
-#if defined(__ANDROID__) || defined(ANDROID)
-  #define EIGEN_OS_ANDROID 1
-#else
-  #define EIGEN_OS_ANDROID 0
-#endif
-
-/// \internal EIGEN_OS_GNULINUX set to 1 if the OS is GNU Linux and not Linux-based OS (e.g., not android)
-#if defined(__gnu_linux__) && !(EIGEN_OS_ANDROID)
-  #define EIGEN_OS_GNULINUX 1
-#else
-  #define EIGEN_OS_GNULINUX 0
-#endif
-
-/// \internal EIGEN_OS_BSD set to 1 if the OS is a BSD variant
-#if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__OpenBSD__) || defined(__bsdi__) || defined(__DragonFly__)
-  #define EIGEN_OS_BSD 1
-#else
-  #define EIGEN_OS_BSD 0
-#endif
-
-/// \internal EIGEN_OS_MAC set to 1 if the OS is MacOS
-#if defined(__APPLE__)
-  #define EIGEN_OS_MAC 1
-#else
-  #define EIGEN_OS_MAC 0
-#endif
-
-/// \internal EIGEN_OS_QNX set to 1 if the OS is QNX
-#if defined(__QNX__)
-  #define EIGEN_OS_QNX 1
-#else
-  #define EIGEN_OS_QNX 0
-#endif
-
-/// \internal EIGEN_OS_WIN set to 1 if the OS is Windows based
-#if defined(_WIN32)
-  #define EIGEN_OS_WIN 1
-#else
-  #define EIGEN_OS_WIN 0
-#endif
-
-/// \internal EIGEN_OS_WIN64 set to 1 if the OS is Windows 64bits
-#if defined(_WIN64)
-  #define EIGEN_OS_WIN64 1
-#else
-  #define EIGEN_OS_WIN64 0
-#endif
-
-/// \internal EIGEN_OS_WINCE set to 1 if the OS is Windows CE
-#if defined(_WIN32_WCE)
-  #define EIGEN_OS_WINCE 1
-#else
-  #define EIGEN_OS_WINCE 0
-#endif
-
-/// \internal EIGEN_OS_CYGWIN set to 1 if the OS is Windows/Cygwin
-#if defined(__CYGWIN__)
-  #define EIGEN_OS_CYGWIN 1
-#else
-  #define EIGEN_OS_CYGWIN 0
-#endif
-
-/// \internal EIGEN_OS_WIN_STRICT set to 1 if the OS is really Windows and not some variants
-#if EIGEN_OS_WIN && !( EIGEN_OS_WINCE || EIGEN_OS_CYGWIN )
-  #define EIGEN_OS_WIN_STRICT 1
-#else
-  #define EIGEN_OS_WIN_STRICT 0
-#endif
-
-/// \internal EIGEN_OS_SUN set to 1 if the OS is SUN
-#if (defined(sun) || defined(__sun)) && !(defined(__SVR4) || defined(__svr4__))
-  #define EIGEN_OS_SUN 1
-#else
-  #define EIGEN_OS_SUN 0
-#endif
-
-/// \internal EIGEN_OS_SOLARIS set to 1 if the OS is Solaris
-#if (defined(sun) || defined(__sun)) && (defined(__SVR4) || defined(__svr4__))
-  #define EIGEN_OS_SOLARIS 1
-#else
-  #define EIGEN_OS_SOLARIS 0
-#endif
-
-
-
-#if EIGEN_GNUC_AT_MOST(4,3) && !EIGEN_COMP_CLANG
-  // see bug 89
-  #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0
-#else
-  #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 1
-#endif
-
-// This macro can be used to prevent from macro expansion, e.g.:
-//   std::max EIGEN_NOT_A_MACRO(a,b)
-#define EIGEN_NOT_A_MACRO
-
-#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
-#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::RowMajor
-#else
-#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::ColMajor
-#endif
-
-#ifndef EIGEN_DEFAULT_DENSE_INDEX_TYPE
-#define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t
-#endif
-
-// Cross compiler wrapper around LLVM's __has_builtin
-#ifdef __has_builtin
-#  define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
-#else
-#  define EIGEN_HAS_BUILTIN(x) 0
-#endif
-
-// A Clang feature extension to determine compiler features.
-// We use it to determine 'cxx_rvalue_references'
-#ifndef __has_feature
-# define __has_feature(x) 0
-#endif
-
-// Upperbound on the C++ version to use.
-// Expected values are 03, 11, 14, 17, etc.
-// By default, let's use an arbitrarily large C++ version.
-#ifndef EIGEN_MAX_CPP_VER
-#define EIGEN_MAX_CPP_VER 99
-#endif
-
-#if EIGEN_MAX_CPP_VER>=11 && (defined(__cplusplus) && (__cplusplus >= 201103L) || EIGEN_COMP_MSVC >= 1900)
-#define EIGEN_HAS_CXX11 1
-#else
-#define EIGEN_HAS_CXX11 0
-#endif
-
-
-// Do we support r-value references?
-#ifndef EIGEN_HAS_RVALUE_REFERENCES
-#if EIGEN_MAX_CPP_VER>=11 && \
-    (__has_feature(cxx_rvalue_references) || \
-    (defined(__cplusplus) && __cplusplus >= 201103L) || \
-    (EIGEN_COMP_MSVC >= 1600))
-  #define EIGEN_HAS_RVALUE_REFERENCES 1
-#else
-  #define EIGEN_HAS_RVALUE_REFERENCES 0
-#endif
-#endif
-
-// Does the compiler support C99?
-#ifndef EIGEN_HAS_C99_MATH
-#if EIGEN_MAX_CPP_VER>=11 && \
-    ((defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901))       \
-  || (defined(__GNUC__) && defined(_GLIBCXX_USE_C99)) \
-  || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)))
-  #define EIGEN_HAS_C99_MATH 1
-#else
-  #define EIGEN_HAS_C99_MATH 0
-#endif
-#endif
-
-// Does the compiler support result_of?
-#ifndef EIGEN_HAS_STD_RESULT_OF
-#if EIGEN_MAX_CPP_VER>=11 && ((__has_feature(cxx_lambdas) || (defined(__cplusplus) && __cplusplus >= 201103L)))
-#define EIGEN_HAS_STD_RESULT_OF 1
-#else
-#define EIGEN_HAS_STD_RESULT_OF 0
-#endif
-#endif
-
-// Does the compiler support variadic templates?
-#ifndef EIGEN_HAS_VARIADIC_TEMPLATES
-#if EIGEN_MAX_CPP_VER>=11 && (__cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900) \
-  && (!defined(__NVCC__) || !EIGEN_ARCH_ARM_OR_ARM64 || (EIGEN_CUDACC_VER >= 80000) )
-    // ^^ Disable the use of variadic templates when compiling with versions of nvcc older than 8.0 on ARM devices:
-    //    this prevents nvcc from crashing when compiling Eigen on Tegra X1
-#define EIGEN_HAS_VARIADIC_TEMPLATES 1
-#else
-#define EIGEN_HAS_VARIADIC_TEMPLATES 0
-#endif
-#endif
-
-// Does the compiler fully support const expressions? (as in c++14)
-#ifndef EIGEN_HAS_CONSTEXPR
-
-#ifdef __CUDACC__
-// Const expressions are supported provided that c++11 is enabled and we're using either clang or nvcc 7.5 or above
-#if EIGEN_MAX_CPP_VER>=14 && (__cplusplus > 199711L && (EIGEN_COMP_CLANG || EIGEN_CUDACC_VER >= 70500))
-  #define EIGEN_HAS_CONSTEXPR 1
-#endif
-#elif EIGEN_MAX_CPP_VER>=14 && (__has_feature(cxx_relaxed_constexpr) || (defined(__cplusplus) && __cplusplus >= 201402L) || \
-  (EIGEN_GNUC_AT_LEAST(4,8) && (__cplusplus > 199711L)))
-#define EIGEN_HAS_CONSTEXPR 1
-#endif
-
-#ifndef EIGEN_HAS_CONSTEXPR
-#define EIGEN_HAS_CONSTEXPR 0
-#endif
-
-#endif
-
-// Does the compiler support C++11 math?
-// Let's be conservative and enable the default C++11 implementation only if we are sure it exists
-#ifndef EIGEN_HAS_CXX11_MATH
-  #if EIGEN_MAX_CPP_VER>=11 && ((__cplusplus > 201103L) || (__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC)  \
-      && (EIGEN_ARCH_i386_OR_x86_64) && (EIGEN_OS_GNULINUX || EIGEN_OS_WIN_STRICT || EIGEN_OS_MAC))
-    #define EIGEN_HAS_CXX11_MATH 1
-  #else
-    #define EIGEN_HAS_CXX11_MATH 0
-  #endif
-#endif
-
-// Does the compiler support proper C++11 containers?
-#ifndef EIGEN_HAS_CXX11_CONTAINERS
-  #if    EIGEN_MAX_CPP_VER>=11 && \
-         ((__cplusplus > 201103L) \
-      || ((__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_ICC>=1400)) \
-      || EIGEN_COMP_MSVC >= 1900)
-    #define EIGEN_HAS_CXX11_CONTAINERS 1
-  #else
-    #define EIGEN_HAS_CXX11_CONTAINERS 0
-  #endif
-#endif
-
-// Does the compiler support C++11 noexcept?
-#ifndef EIGEN_HAS_CXX11_NOEXCEPT
-  #if    EIGEN_MAX_CPP_VER>=11 && \
-         (__has_feature(cxx_noexcept) \
-      || (__cplusplus > 201103L) \
-      || ((__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_ICC>=1400)) \
-      || EIGEN_COMP_MSVC >= 1900)
-    #define EIGEN_HAS_CXX11_NOEXCEPT 1
-  #else
-    #define EIGEN_HAS_CXX11_NOEXCEPT 0
-  #endif
-#endif
-
-/** Allows to disable some optimizations which might affect the accuracy of the result.
-  * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
-  * They currently include:
-  *   - single precision ArrayBase::sin() and ArrayBase::cos() for SSE and AVX vectorization.
-  */
-#ifndef EIGEN_FAST_MATH
-#define EIGEN_FAST_MATH 1
-#endif
-
-#define EIGEN_DEBUG_VAR(x) std::cerr << #x << " = " << x << std::endl;
-
-// concatenate two tokens
-#define EIGEN_CAT2(a,b) a ## b
-#define EIGEN_CAT(a,b) EIGEN_CAT2(a,b)
-
-#define EIGEN_COMMA ,
-
-// convert a token to a string
-#define EIGEN_MAKESTRING2(a) #a
-#define EIGEN_MAKESTRING(a) EIGEN_MAKESTRING2(a)
-
-// EIGEN_STRONG_INLINE is a stronger version of the inline, using __forceinline on MSVC,
-// but it still doesn't use GCC's always_inline. This is useful in (common) situations where MSVC needs forceinline
-// but GCC is still doing fine with just inline.
-#ifndef EIGEN_STRONG_INLINE
-#if EIGEN_COMP_MSVC || EIGEN_COMP_ICC
-#define EIGEN_STRONG_INLINE __forceinline
-#else
-#define EIGEN_STRONG_INLINE inline
-#endif
-#endif
-
-// EIGEN_ALWAYS_INLINE is the stronget, it has the effect of making the function inline and adding every possible
-// attribute to maximize inlining. This should only be used when really necessary: in particular,
-// it uses __attribute__((always_inline)) on GCC, which most of the time is useless and can severely harm compile times.
-// FIXME with the always_inline attribute,
-// gcc 3.4.x and 4.1 reports the following compilation error:
-//   Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval<Derived> Eigen::MatrixBase<Scalar, Derived>::eval() const'
-//    : function body not available
-//   See also bug 1367
-#if EIGEN_GNUC_AT_LEAST(4,2)
-#define EIGEN_ALWAYS_INLINE __attribute__((always_inline)) inline
-#else
-#define EIGEN_ALWAYS_INLINE EIGEN_STRONG_INLINE
-#endif
-
-#if EIGEN_COMP_GNUC
-#define EIGEN_DONT_INLINE __attribute__((noinline))
-#elif EIGEN_COMP_MSVC
-#define EIGEN_DONT_INLINE __declspec(noinline)
-#else
-#define EIGEN_DONT_INLINE
-#endif
-
-#if EIGEN_COMP_GNUC
-#define EIGEN_PERMISSIVE_EXPR __extension__
-#else
-#define EIGEN_PERMISSIVE_EXPR
-#endif
-
-// this macro allows to get rid of linking errors about multiply defined functions.
-//  - static is not very good because it prevents definitions from different object files to be merged.
-//           So static causes the resulting linked executable to be bloated with multiple copies of the same function.
-//  - inline is not perfect either as it unwantedly hints the compiler toward inlining the function.
-#define EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-#define EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS inline
-
-#ifdef NDEBUG
-# ifndef EIGEN_NO_DEBUG
-#  define EIGEN_NO_DEBUG
-# endif
-#endif
-
-// eigen_plain_assert is where we implement the workaround for the assert() bug in GCC <= 4.3, see bug 89
-#ifdef EIGEN_NO_DEBUG
-  #define eigen_plain_assert(x)
-#else
-  #if EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO
-    namespace Eigen {
-    namespace internal {
-    inline bool copy_bool(bool b) { return b; }
-    }
-    }
-    #define eigen_plain_assert(x) assert(x)
-  #else
-    // work around bug 89
-    #include <cstdlib>   // for abort
-    #include <iostream>  // for std::cerr
-
-    namespace Eigen {
-    namespace internal {
-    // trivial function copying a bool. Must be EIGEN_DONT_INLINE, so we implement it after including Eigen headers.
-    // see bug 89.
-    namespace {
-    EIGEN_DONT_INLINE bool copy_bool(bool b) { return b; }
-    }
-    inline void assert_fail(const char *condition, const char *function, const char *file, int line)
-    {
-      std::cerr << "assertion failed: " << condition << " in function " << function << " at " << file << ":" << line << std::endl;
-      abort();
-    }
-    }
-    }
-    #define eigen_plain_assert(x) \
-      do { \
-        if(!Eigen::internal::copy_bool(x)) \
-          Eigen::internal::assert_fail(EIGEN_MAKESTRING(x), __PRETTY_FUNCTION__, __FILE__, __LINE__); \
-      } while(false)
-  #endif
-#endif
-
-// eigen_assert can be overridden
-#ifndef eigen_assert
-#define eigen_assert(x) eigen_plain_assert(x)
-#endif
-
-#ifdef EIGEN_INTERNAL_DEBUGGING
-#define eigen_internal_assert(x) eigen_assert(x)
-#else
-#define eigen_internal_assert(x)
-#endif
-
-#ifdef EIGEN_NO_DEBUG
-#define EIGEN_ONLY_USED_FOR_DEBUG(x) EIGEN_UNUSED_VARIABLE(x)
-#else
-#define EIGEN_ONLY_USED_FOR_DEBUG(x)
-#endif
-
-#ifndef EIGEN_NO_DEPRECATED_WARNING
-  #if EIGEN_COMP_GNUC
-    #define EIGEN_DEPRECATED __attribute__((deprecated))
-  #elif EIGEN_COMP_MSVC
-    #define EIGEN_DEPRECATED __declspec(deprecated)
-  #else
-    #define EIGEN_DEPRECATED
-  #endif
-#else
-  #define EIGEN_DEPRECATED
-#endif
-
-#if EIGEN_COMP_GNUC
-#define EIGEN_UNUSED __attribute__((unused))
-#else
-#define EIGEN_UNUSED
-#endif
-
-// Suppresses 'unused variable' warnings.
-namespace Eigen {
-  namespace internal {
-    template<typename T> EIGEN_DEVICE_FUNC void ignore_unused_variable(const T&) {}
-  }
-}
-#define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var);
-
-#if !defined(EIGEN_ASM_COMMENT)
-  #if EIGEN_COMP_GNUC && (EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64)
-    #define EIGEN_ASM_COMMENT(X)  __asm__("#" X)
-  #else
-    #define EIGEN_ASM_COMMENT(X)
-  #endif
-#endif
-
-
-//------------------------------------------------------------------------------------------
-// Static and dynamic alignment control
-//
-// The main purpose of this section is to define EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES
-// as the maximal boundary in bytes on which dynamically and statically allocated data may be alignment respectively.
-// The values of EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES can be specified by the user. If not,
-// a default value is automatically computed based on architecture, compiler, and OS.
-//
-// This section also defines macros EIGEN_ALIGN_TO_BOUNDARY(N) and the shortcuts EIGEN_ALIGN{8,16,32,_MAX}
-// to be used to declare statically aligned buffers.
-//------------------------------------------------------------------------------------------
-
-
-/* EIGEN_ALIGN_TO_BOUNDARY(n) forces data to be n-byte aligned. This is used to satisfy SIMD requirements.
- * However, we do that EVEN if vectorization (EIGEN_VECTORIZE) is disabled,
- * so that vectorization doesn't affect binary compatibility.
- *
- * If we made alignment depend on whether or not EIGEN_VECTORIZE is defined, it would be impossible to link
- * vectorized and non-vectorized code.
- */
-#if (defined __CUDACC__)
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __align__(n)
-#elif EIGEN_COMP_GNUC || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
-#elif EIGEN_COMP_MSVC
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __declspec(align(n))
-#elif EIGEN_COMP_SUNCC
-  // FIXME not sure about this one:
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
-#else
-  #error Please tell me what is the equivalent of __attribute__((aligned(n))) for your compiler
-#endif
-
-// If the user explicitly disable vectorization, then we also disable alignment
-#if defined(EIGEN_DONT_VECTORIZE)
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 0
-#elif defined(EIGEN_VECTORIZE_AVX512)
-  // 64 bytes static alignmeent is preferred only if really required
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 64
-#elif defined(__AVX__)
-  // 32 bytes static alignmeent is preferred only if really required
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 32
-#else
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
-#endif
-
-
-// EIGEN_MIN_ALIGN_BYTES defines the minimal value for which the notion of explicit alignment makes sense
-#define EIGEN_MIN_ALIGN_BYTES 16
-
-// Defined the boundary (in bytes) on which the data needs to be aligned. Note
-// that unless EIGEN_ALIGN is defined and not equal to 0, the data may not be
-// aligned at all regardless of the value of this #define.
-
-#if (defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN))  && defined(EIGEN_MAX_STATIC_ALIGN_BYTES) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
-#error EIGEN_MAX_STATIC_ALIGN_BYTES and EIGEN_DONT_ALIGN[_STATICALLY] are both defined with EIGEN_MAX_STATIC_ALIGN_BYTES!=0. Use EIGEN_MAX_STATIC_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN_STATICALLY.
-#endif
-
-// EIGEN_DONT_ALIGN_STATICALLY and EIGEN_DONT_ALIGN are deprectated
-// They imply EIGEN_MAX_STATIC_ALIGN_BYTES=0
-#if defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN)
-  #ifdef EIGEN_MAX_STATIC_ALIGN_BYTES
-    #undef EIGEN_MAX_STATIC_ALIGN_BYTES
-  #endif
-  #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
-#endif
-
-#ifndef EIGEN_MAX_STATIC_ALIGN_BYTES
-
-  // Try to automatically guess what is the best default value for EIGEN_MAX_STATIC_ALIGN_BYTES
-
-  // 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
-  // 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
-  // enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
-  // certain common platform (compiler+architecture combinations) to avoid these problems.
-  // Only static alignment is really problematic (relies on nonstandard compiler extensions),
-  // try to keep heap alignment even when we have to disable static alignment.
-  #if EIGEN_COMP_GNUC && !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64)
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
-  #elif EIGEN_ARCH_ARM_OR_ARM64 && EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_MOST(4, 6)
-  // Old versions of GCC on ARM, at least 4.4, were once seen to have buggy static alignment support.
-  // Not sure which version fixed it, hopefully it doesn't affect 4.7, which is still somewhat in use.
-  // 4.8 and newer seem definitely unaffected.
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
-  #else
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 0
-  #endif
-
-  // static alignment is completely disabled with GCC 3, Sun Studio, and QCC/QNX
-  #if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT \
-  && !EIGEN_GCC3_OR_OLDER \
-  && !EIGEN_COMP_SUNCC \
-  && !EIGEN_OS_QNX
-    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 1
-  #else
-    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 0
-  #endif
-
-  #if EIGEN_ARCH_WANTS_STACK_ALIGNMENT
-    #define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
-  #else
-    #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
-  #endif
-
-#endif
-
-// If EIGEN_MAX_ALIGN_BYTES is defined, then it is considered as an upper bound for EIGEN_MAX_ALIGN_BYTES
-#if defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES<EIGEN_MAX_STATIC_ALIGN_BYTES
-#undef EIGEN_MAX_STATIC_ALIGN_BYTES
-#define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
-#endif
-
-#if EIGEN_MAX_STATIC_ALIGN_BYTES==0 && !defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
-  #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
-#endif
-
-// At this stage, EIGEN_MAX_STATIC_ALIGN_BYTES>0 is the true test whether we want to align arrays on the stack or not.
-// It takes into account both the user choice to explicitly enable/disable alignment (by settting EIGEN_MAX_STATIC_ALIGN_BYTES)
-// and the architecture config (EIGEN_ARCH_WANTS_STACK_ALIGNMENT).
-// Henceforth, only EIGEN_MAX_STATIC_ALIGN_BYTES should be used.
-
-
-// Shortcuts to EIGEN_ALIGN_TO_BOUNDARY
-#define EIGEN_ALIGN8  EIGEN_ALIGN_TO_BOUNDARY(8)
-#define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16)
-#define EIGEN_ALIGN32 EIGEN_ALIGN_TO_BOUNDARY(32)
-#define EIGEN_ALIGN64 EIGEN_ALIGN_TO_BOUNDARY(64)
-#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
-#define EIGEN_ALIGN_MAX EIGEN_ALIGN_TO_BOUNDARY(EIGEN_MAX_STATIC_ALIGN_BYTES)
-#else
-#define EIGEN_ALIGN_MAX
-#endif
-
-
-// Dynamic alignment control
-
-#if defined(EIGEN_DONT_ALIGN) && defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES>0
-#error EIGEN_MAX_ALIGN_BYTES and EIGEN_DONT_ALIGN are both defined with EIGEN_MAX_ALIGN_BYTES!=0. Use EIGEN_MAX_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN.
-#endif
-
-#ifdef EIGEN_DONT_ALIGN
-  #ifdef EIGEN_MAX_ALIGN_BYTES
-    #undef EIGEN_MAX_ALIGN_BYTES
-  #endif
-  #define EIGEN_MAX_ALIGN_BYTES 0
-#elif !defined(EIGEN_MAX_ALIGN_BYTES)
-  #define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
-#endif
-
-#if EIGEN_IDEAL_MAX_ALIGN_BYTES > EIGEN_MAX_ALIGN_BYTES
-#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
-#else
-#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
-#endif
-
-
-#ifndef EIGEN_UNALIGNED_VECTORIZE
-#define EIGEN_UNALIGNED_VECTORIZE 1
-#endif
-
-//----------------------------------------------------------------------
-
-
-#ifdef EIGEN_DONT_USE_RESTRICT_KEYWORD
-  #define EIGEN_RESTRICT
-#endif
-#ifndef EIGEN_RESTRICT
-  #define EIGEN_RESTRICT __restrict
-#endif
-
-#ifndef EIGEN_STACK_ALLOCATION_LIMIT
-// 131072 == 128 KB
-#define EIGEN_STACK_ALLOCATION_LIMIT 131072
-#endif
-
-#ifndef EIGEN_DEFAULT_IO_FORMAT
-#ifdef EIGEN_MAKING_DOCS
-// format used in Eigen's documentation
-// needed to define it here as escaping characters in CMake add_definition's argument seems very problematic.
-#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(3, 0, " ", "\n", "", "")
-#else
-#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat()
-#endif
-#endif
-
-// just an empty macro !
-#define EIGEN_EMPTY
-
-#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC < 1900 || EIGEN_CUDACC_VER>0)
-  // for older MSVC versions, as well as 1900 && CUDA 8, using the base operator is sufficient (cf Bugs 1000, 1324)
-  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
-    using Base::operator =;
-#elif EIGEN_COMP_CLANG // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)
-  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
-    using Base::operator =; \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) { Base::operator=(other); return *this; } \
-    template <typename OtherDerived> \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const DenseBase<OtherDerived>& other) { Base::operator=(other.derived()); return *this; }
-#else
-  #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
-    using Base::operator =; \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const Derived& other) \
-    { \
-      Base::operator=(other); \
-      return *this; \
-    }
-#endif
-
-
-/** \internal
- * \brief Macro to manually inherit assignment operators.
- * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined.
- */
-#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
-
-/**
-* Just a side note. Commenting within defines works only by documenting
-* behind the object (via '!<'). Comments cannot be multi-line and thus
-* we have these extra long lines. What is confusing doxygen over here is
-* that we use '\' and basically have a bunch of typedefs with their
-* documentation in a single line.
-**/
-
-#define EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) \
-  typedef typename Eigen::internal::traits<Derived>::Scalar Scalar; /*!< \brief Numeric type, e.g. float, double, int or std::complex<float>. */ \
-  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; /*!< \brief The underlying numeric type for composed scalar types. \details In cases where Scalar is e.g. std::complex<T>, T were corresponding to RealScalar. */ \
-  typedef typename Base::CoeffReturnType CoeffReturnType; /*!< \brief The return type for coefficient access. \details Depending on whether the object allows direct coefficient access (e.g. for a MatrixXd), this type is either 'const Scalar&' or simply 'Scalar' for objects that do not allow direct coefficient access. */ \
-  typedef typename Eigen::internal::ref_selector<Derived>::type Nested; \
-  typedef typename Eigen::internal::traits<Derived>::StorageKind StorageKind; \
-  typedef typename Eigen::internal::traits<Derived>::StorageIndex StorageIndex; \
-  enum { RowsAtCompileTime = Eigen::internal::traits<Derived>::RowsAtCompileTime, \
-        ColsAtCompileTime = Eigen::internal::traits<Derived>::ColsAtCompileTime, \
-        Flags = Eigen::internal::traits<Derived>::Flags, \
-        SizeAtCompileTime = Base::SizeAtCompileTime, \
-        MaxSizeAtCompileTime = Base::MaxSizeAtCompileTime, \
-        IsVectorAtCompileTime = Base::IsVectorAtCompileTime }; \
-  using Base::derived; \
-  using Base::const_cast_derived;
-
-
-// FIXME Maybe the EIGEN_DENSE_PUBLIC_INTERFACE could be removed as importing PacketScalar is rarely needed
-#define EIGEN_DENSE_PUBLIC_INTERFACE(Derived) \
-  EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) \
-  typedef typename Base::PacketScalar PacketScalar;
-
-
-#define EIGEN_PLAIN_ENUM_MIN(a,b) (((int)a <= (int)b) ? (int)a : (int)b)
-#define EIGEN_PLAIN_ENUM_MAX(a,b) (((int)a >= (int)b) ? (int)a : (int)b)
-
-// EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
-// followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
-// finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
-#define EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) (((int)a == 0 || (int)b == 0) ? 0 \
-                           : ((int)a == 1 || (int)b == 1) ? 1 \
-                           : ((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \
-                           : ((int)a <= (int)b) ? (int)a : (int)b)
-
-// EIGEN_SIZE_MIN_PREFER_FIXED is a variant of EIGEN_SIZE_MIN_PREFER_DYNAMIC comparing MaxSizes. The difference is that finite values
-// now have priority over Dynamic, so that min(3, Dynamic) gives 3. Indeed, whatever the actual value is
-// (between 0 and 3), it is not more than 3.
-#define EIGEN_SIZE_MIN_PREFER_FIXED(a,b)  (((int)a == 0 || (int)b == 0) ? 0 \
-                           : ((int)a == 1 || (int)b == 1) ? 1 \
-                           : ((int)a == Dynamic && (int)b == Dynamic) ? Dynamic \
-                           : ((int)a == Dynamic) ? (int)b \
-                           : ((int)b == Dynamic) ? (int)a \
-                           : ((int)a <= (int)b) ? (int)a : (int)b)
-
-// see EIGEN_SIZE_MIN_PREFER_DYNAMIC. No need for a separate variant for MaxSizes here.
-#define EIGEN_SIZE_MAX(a,b) (((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \
-                           : ((int)a >= (int)b) ? (int)a : (int)b)
-
-#define EIGEN_LOGICAL_XOR(a,b) (((a) || (b)) && !((a) && (b)))
-
-#define EIGEN_IMPLIES(a,b) (!(a) || (b))
-
-// the expression type of a standard coefficient wise binary operation
-#define EIGEN_CWISE_BINARY_RETURN_TYPE(LHS,RHS,OPNAME) \
-    CwiseBinaryOp< \
-      EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)< \
-          typename internal::traits<LHS>::Scalar, \
-          typename internal::traits<RHS>::Scalar \
-      >, \
-      const LHS, \
-      const RHS \
-    >
-
-#define EIGEN_MAKE_CWISE_BINARY_OP(METHOD,OPNAME) \
-  template<typename OtherDerived> \
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,OPNAME) \
-  (METHOD)(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const \
-  { \
-    return EIGEN_CWISE_BINARY_RETURN_TYPE(Derived,OtherDerived,OPNAME)(derived(), other.derived()); \
-  }
-
-#define EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,TYPEA,TYPEB) \
-  (Eigen::internal::has_ReturnType<Eigen::ScalarBinaryOpTraits<TYPEA,TYPEB,EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,OPNAME),_op)<TYPEA,TYPEB>  > >::value)
-
-#define EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(EXPR,SCALAR,OPNAME) \
-  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)<typename internal::traits<EXPR>::Scalar,SCALAR>, const EXPR, \
-                const typename internal::plain_constant_type<EXPR,SCALAR>::type>
-
-#define EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(SCALAR,EXPR,OPNAME) \
-  CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)<SCALAR,typename internal::traits<EXPR>::Scalar>, \
-                const typename internal::plain_constant_type<EXPR,SCALAR>::type, const EXPR>
-
-// Workaround for MSVC 2010 (see ML thread "patch with compile for for MSVC 2010")
-#if EIGEN_COMP_MSVC_STRICT<=1600
-#define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) typename internal::enable_if<true,X>::type
-#else
-#define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) X
-#endif
-
-#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME) \
-  template <typename T> EIGEN_DEVICE_FUNC inline \
-  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type,OPNAME))\
-  (METHOD)(const T& scalar) const { \
-    typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type PromotedT; \
-    return EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,PromotedT,OPNAME)(derived(), \
-           typename internal::plain_constant_type<Derived,PromotedT>::type(derived().rows(), derived().cols(), internal::scalar_constant_op<PromotedT>(scalar))); \
-  }
-
-#define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD,OPNAME) \
-  template <typename T> EIGEN_DEVICE_FUNC inline friend \
-  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type,Derived,OPNAME)) \
-  (METHOD)(const T& scalar, const StorageBaseType& matrix) { \
-    typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type PromotedT; \
-    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedT,Derived,OPNAME)( \
-           typename internal::plain_constant_type<Derived,PromotedT>::type(matrix.derived().rows(), matrix.derived().cols(), internal::scalar_constant_op<PromotedT>(scalar)), matrix.derived()); \
-  }
-
-#define EIGEN_MAKE_SCALAR_BINARY_OP(METHOD,OPNAME) \
-  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD,OPNAME) \
-  EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME)
-
-
-#ifdef EIGEN_EXCEPTIONS
-#  define EIGEN_THROW_X(X) throw X
-#  define EIGEN_THROW throw
-#  define EIGEN_TRY try
-#  define EIGEN_CATCH(X) catch (X)
-#else
-#  ifdef __CUDA_ARCH__
-#    define EIGEN_THROW_X(X) asm("trap;")
-#    define EIGEN_THROW asm("trap;")
-#  else
-#    define EIGEN_THROW_X(X) std::abort()
-#    define EIGEN_THROW std::abort()
-#  endif
-#  define EIGEN_TRY if (true)
-#  define EIGEN_CATCH(X) else
-#endif
-
-
-#if EIGEN_HAS_CXX11_NOEXCEPT
-#   define EIGEN_INCLUDE_TYPE_TRAITS
-#   define EIGEN_NOEXCEPT noexcept
-#   define EIGEN_NOEXCEPT_IF(x) noexcept(x)
-#   define EIGEN_NO_THROW noexcept(true)
-#   define EIGEN_EXCEPTION_SPEC(X) noexcept(false)
-#else
-#   define EIGEN_NOEXCEPT
-#   define EIGEN_NOEXCEPT_IF(x)
-#   define EIGEN_NO_THROW throw()
-#   if EIGEN_COMP_MSVC
-      // MSVC does not support exception specifications (warning C4290),
-      // and they are deprecated in c++11 anyway.
-#     define EIGEN_EXCEPTION_SPEC(X) throw()
-#   else
-#     define EIGEN_EXCEPTION_SPEC(X) throw(X)
-#   endif
-#endif
-
-#endif // EIGEN_MACROS_H

wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h

diff --git a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h b/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
deleted file mode 100644
index ba5bd18..0000000
--- a/wpiutil/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
+++ /dev/null
@@ -1,821 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_XPRHELPER_H
-#define EIGEN_XPRHELPER_H
-
-// just a workaround because GCC seems to not really like empty structs
-// FIXME: gcc 4.3 generates bad code when strict-aliasing is enabled
-// so currently we simply disable this optimization for gcc 4.3
-#if EIGEN_COMP_GNUC && !EIGEN_GNUC_AT(4,3)
-  #define EIGEN_EMPTY_STRUCT_CTOR(X) \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE X() {} \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE X(const X& ) {}
-#else
-  #define EIGEN_EMPTY_STRUCT_CTOR(X)
-#endif
-
-namespace Eigen {
-
-namespace internal {
-
-template<typename IndexDest, typename IndexSrc>
-EIGEN_DEVICE_FUNC
-inline IndexDest convert_index(const IndexSrc& idx) {
-  // for sizeof(IndexDest)>=sizeof(IndexSrc) compilers should be able to optimize this away:
-  eigen_internal_assert(idx <= NumTraits<IndexDest>::highest() && "Index value to big for target type");
-  return IndexDest(idx);
-}
-
-
-// promote_scalar_arg is an helper used in operation between an expression and a scalar, like:
-//    expression * scalar
-// Its role is to determine how the type T of the scalar operand should be promoted given the scalar type ExprScalar of the given expression.
-// The IsSupported template parameter must be provided by the caller as: internal::has_ReturnType<ScalarBinaryOpTraits<ExprScalar,T,op> >::value using the proper order for ExprScalar and T.
-// Then the logic is as follows:
-//  - if the operation is natively supported as defined by IsSupported, then the scalar type is not promoted, and T is returned.
-//  - otherwise, NumTraits<ExprScalar>::Literal is returned if T is implicitly convertible to NumTraits<ExprScalar>::Literal AND that this does not imply a float to integer conversion.
-//  - otherwise, ExprScalar is returned if T is implicitly convertible to ExprScalar AND that this does not imply a float to integer conversion.
-//  - In all other cases, the promoted type is not defined, and the respective operation is thus invalid and not available (SFINAE).
-template<typename ExprScalar,typename T, bool IsSupported>
-struct promote_scalar_arg;
-
-template<typename S,typename T>
-struct promote_scalar_arg<S,T,true>
-{
-  typedef T type;
-};
-
-// Recursively check safe conversion to PromotedType, and then ExprScalar if they are different.
-template<typename ExprScalar,typename T,typename PromotedType,
-  bool ConvertibleToLiteral = internal::is_convertible<T,PromotedType>::value,
-  bool IsSafe = NumTraits<T>::IsInteger || !NumTraits<PromotedType>::IsInteger>
-struct promote_scalar_arg_unsupported;
-
-// Start recursion with NumTraits<ExprScalar>::Literal
-template<typename S,typename T>
-struct promote_scalar_arg<S,T,false> : promote_scalar_arg_unsupported<S,T,typename NumTraits<S>::Literal> {};
-
-// We found a match!
-template<typename S,typename T, typename PromotedType>
-struct promote_scalar_arg_unsupported<S,T,PromotedType,true,true>
-{
-  typedef PromotedType type;
-};
-
-// No match, but no real-to-integer issues, and ExprScalar and current PromotedType are different,
-// so let's try to promote to ExprScalar
-template<typename ExprScalar,typename T, typename PromotedType>
-struct promote_scalar_arg_unsupported<ExprScalar,T,PromotedType,false,true>
-   : promote_scalar_arg_unsupported<ExprScalar,T,ExprScalar>
-{};
-
-// Unsafe real-to-integer, let's stop.
-template<typename S,typename T, typename PromotedType, bool ConvertibleToLiteral>
-struct promote_scalar_arg_unsupported<S,T,PromotedType,ConvertibleToLiteral,false> {};
-
-// T is not even convertible to ExprScalar, let's stop.
-template<typename S,typename T>
-struct promote_scalar_arg_unsupported<S,T,S,false,true> {};
-
-//classes inheriting no_assignment_operator don't generate a default operator=.
-class no_assignment_operator
-{
-  private:
-    no_assignment_operator& operator=(const no_assignment_operator&);
-};
-
-/** \internal return the index type with the largest number of bits */
-template<typename I1, typename I2>
-struct promote_index_type
-{
-  typedef typename conditional<(sizeof(I1)<sizeof(I2)), I2, I1>::type type;
-};
-
-/** \internal If the template parameter Value is Dynamic, this class is just a wrapper around a T variable that
-  * can be accessed using value() and setValue().
-  * Otherwise, this class is an empty structure and value() just returns the template parameter Value.
-  */
-template<typename T, int Value> class variable_if_dynamic
-{
-  public:
-    EIGEN_EMPTY_STRUCT_CTOR(variable_if_dynamic)
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
-    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T value() { return T(Value); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {}
-};
-
-template<typename T> class variable_if_dynamic<T, Dynamic>
-{
-    T m_value;
-    EIGEN_DEVICE_FUNC variable_if_dynamic() { eigen_assert(false); }
-  public:
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value) : m_value(value) {}
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T value() const { return m_value; }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
-};
-
-/** \internal like variable_if_dynamic but for DynamicIndex
-  */
-template<typename T, int Value> class variable_if_dynamicindex
-{
-  public:
-    EIGEN_EMPTY_STRUCT_CTOR(variable_if_dynamicindex)
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
-    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T value() { return T(Value); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {}
-};
-
-template<typename T> class variable_if_dynamicindex<T, DynamicIndex>
-{
-    T m_value;
-    EIGEN_DEVICE_FUNC variable_if_dynamicindex() { eigen_assert(false); }
-  public:
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T value) : m_value(value) {}
-    EIGEN_DEVICE_FUNC T EIGEN_STRONG_INLINE value() const { return m_value; }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
-};
-
-template<typename T> struct functor_traits
-{
-  enum
-  {
-    Cost = 10,
-    PacketAccess = false,
-    IsRepeatable = false
-  };
-};
-
-template<typename T> struct packet_traits;
-
-template<typename T> struct unpacket_traits
-{
-  typedef T type;
-  typedef T half;
-  enum
-  {
-    size = 1,
-    alignment = 1
-  };
-};
-
-template<int Size, typename PacketType,
-         bool Stop = Size==Dynamic || (Size%unpacket_traits<PacketType>::size)==0 || is_same<PacketType,typename unpacket_traits<PacketType>::half>::value>
-struct find_best_packet_helper;
-
-template< int Size, typename PacketType>
-struct find_best_packet_helper<Size,PacketType,true>
-{
-  typedef PacketType type;
-};
-
-template<int Size, typename PacketType>
-struct find_best_packet_helper<Size,PacketType,false>
-{
-  typedef typename find_best_packet_helper<Size,typename unpacket_traits<PacketType>::half>::type type;
-};
-
-template<typename T, int Size>
-struct find_best_packet
-{
-  typedef typename find_best_packet_helper<Size,typename packet_traits<T>::type>::type type;
-};
-
-#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
-template<int ArrayBytes, int AlignmentBytes,
-         bool Match     =  bool((ArrayBytes%AlignmentBytes)==0),
-         bool TryHalf   =  bool(EIGEN_MIN_ALIGN_BYTES<AlignmentBytes) >
-struct compute_default_alignment_helper
-{
-  enum { value = 0 };
-};
-
-template<int ArrayBytes, int AlignmentBytes, bool TryHalf>
-struct compute_default_alignment_helper<ArrayBytes, AlignmentBytes, true, TryHalf> // Match
-{
-  enum { value = AlignmentBytes };
-};
-
-template<int ArrayBytes, int AlignmentBytes>
-struct compute_default_alignment_helper<ArrayBytes, AlignmentBytes, false, true> // Try-half
-{
-  // current packet too large, try with an half-packet
-  enum { value = compute_default_alignment_helper<ArrayBytes, AlignmentBytes/2>::value };
-};
-#else
-// If static alignment is disabled, no need to bother.
-// This also avoids a division by zero in "bool Match =  bool((ArrayBytes%AlignmentBytes)==0)"
-template<int ArrayBytes, int AlignmentBytes>
-struct compute_default_alignment_helper
-{
-  enum { value = 0 };
-};
-#endif
-
-template<typename T, int Size> struct compute_default_alignment {
-  enum { value = compute_default_alignment_helper<Size*sizeof(T),EIGEN_MAX_STATIC_ALIGN_BYTES>::value };
-};
-
-template<typename T> struct compute_default_alignment<T,Dynamic> {
-  enum { value = EIGEN_MAX_ALIGN_BYTES };
-};
-
-template<typename _Scalar, int _Rows, int _Cols,
-         int _Options = AutoAlign |
-                          ( (_Rows==1 && _Cols!=1) ? RowMajor
-                          : (_Cols==1 && _Rows!=1) ? ColMajor
-                          : EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ),
-         int _MaxRows = _Rows,
-         int _MaxCols = _Cols
-> class make_proper_matrix_type
-{
-    enum {
-      IsColVector = _Cols==1 && _Rows!=1,
-      IsRowVector = _Rows==1 && _Cols!=1,
-      Options = IsColVector ? (_Options | ColMajor) & ~RowMajor
-              : IsRowVector ? (_Options | RowMajor) & ~ColMajor
-              : _Options
-    };
-  public:
-    typedef Matrix<_Scalar, _Rows, _Cols, Options, _MaxRows, _MaxCols> type;
-};
-
-template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-class compute_matrix_flags
-{
-    enum { row_major_bit = Options&RowMajor ? RowMajorBit : 0 };
-  public:
-    // FIXME currently we still have to handle DirectAccessBit at the expression level to handle DenseCoeffsBase<>
-    // and then propagate this information to the evaluator's flags.
-    // However, I (Gael) think that DirectAccessBit should only matter at the evaluation stage.
-    enum { ret = DirectAccessBit | LvalueBit | NestByRefBit | row_major_bit };
-};
-
-template<int _Rows, int _Cols> struct size_at_compile_time
-{
-  enum { ret = (_Rows==Dynamic || _Cols==Dynamic) ? Dynamic : _Rows * _Cols };
-};
-
-template<typename XprType> struct size_of_xpr_at_compile_time
-{
-  enum { ret = size_at_compile_time<traits<XprType>::RowsAtCompileTime,traits<XprType>::ColsAtCompileTime>::ret };
-};
-
-/* plain_matrix_type : the difference from eval is that plain_matrix_type is always a plain matrix type,
- * whereas eval is a const reference in the case of a matrix
- */
-
-template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct plain_matrix_type;
-template<typename T, typename BaseClassType, int Flags> struct plain_matrix_type_dense;
-template<typename T> struct plain_matrix_type<T,Dense>
-{
-  typedef typename plain_matrix_type_dense<T,typename traits<T>::XprKind, traits<T>::Flags>::type type;
-};
-template<typename T> struct plain_matrix_type<T,DiagonalShape>
-{
-  typedef typename T::PlainObject type;
-};
-
-template<typename T, int Flags> struct plain_matrix_type_dense<T,MatrixXpr,Flags>
-{
-  typedef Matrix<typename traits<T>::Scalar,
-                traits<T>::RowsAtCompileTime,
-                traits<T>::ColsAtCompileTime,
-                AutoAlign | (Flags&RowMajorBit ? RowMajor : ColMajor),
-                traits<T>::MaxRowsAtCompileTime,
-                traits<T>::MaxColsAtCompileTime
-          > type;
-};
-
-template<typename T, int Flags> struct plain_matrix_type_dense<T,ArrayXpr,Flags>
-{
-  typedef Array<typename traits<T>::Scalar,
-                traits<T>::RowsAtCompileTime,
-                traits<T>::ColsAtCompileTime,
-                AutoAlign | (Flags&RowMajorBit ? RowMajor : ColMajor),
-                traits<T>::MaxRowsAtCompileTime,
-                traits<T>::MaxColsAtCompileTime
-          > type;
-};
-
-/* eval : the return type of eval(). For matrices, this is just a const reference
- * in order to avoid a useless copy
- */
-
-template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct eval;
-
-template<typename T> struct eval<T,Dense>
-{
-  typedef typename plain_matrix_type<T>::type type;
-//   typedef typename T::PlainObject type;
-//   typedef T::Matrix<typename traits<T>::Scalar,
-//                 traits<T>::RowsAtCompileTime,
-//                 traits<T>::ColsAtCompileTime,
-//                 AutoAlign | (traits<T>::Flags&RowMajorBit ? RowMajor : ColMajor),
-//                 traits<T>::MaxRowsAtCompileTime,
-//                 traits<T>::MaxColsAtCompileTime
-//           > type;
-};
-
-template<typename T> struct eval<T,DiagonalShape>
-{
-  typedef typename plain_matrix_type<T>::type type;
-};
-
-// for matrices, no need to evaluate, just use a const reference to avoid a useless copy
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct eval<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, Dense>
-{
-  typedef const Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& type;
-};
-
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct eval<Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, Dense>
-{
-  typedef const Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& type;
-};
-
-
-/* similar to plain_matrix_type, but using the evaluator's Flags */
-template<typename T, typename StorageKind = typename traits<T>::StorageKind> struct plain_object_eval;
-
-template<typename T>
-struct plain_object_eval<T,Dense>
-{
-  typedef typename plain_matrix_type_dense<T,typename traits<T>::XprKind, evaluator<T>::Flags>::type type;
-};
-
-
-/* plain_matrix_type_column_major : same as plain_matrix_type but guaranteed to be column-major
- */
-template<typename T> struct plain_matrix_type_column_major
-{
-  enum { Rows = traits<T>::RowsAtCompileTime,
-         Cols = traits<T>::ColsAtCompileTime,
-         MaxRows = traits<T>::MaxRowsAtCompileTime,
-         MaxCols = traits<T>::MaxColsAtCompileTime
-  };
-  typedef Matrix<typename traits<T>::Scalar,
-                Rows,
-                Cols,
-                (MaxRows==1&&MaxCols!=1) ? RowMajor : ColMajor,
-                MaxRows,
-                MaxCols
-          > type;
-};
-
-/* plain_matrix_type_row_major : same as plain_matrix_type but guaranteed to be row-major
- */
-template<typename T> struct plain_matrix_type_row_major
-{
-  enum { Rows = traits<T>::RowsAtCompileTime,
-         Cols = traits<T>::ColsAtCompileTime,
-         MaxRows = traits<T>::MaxRowsAtCompileTime,
-         MaxCols = traits<T>::MaxColsAtCompileTime
-  };
-  typedef Matrix<typename traits<T>::Scalar,
-                Rows,
-                Cols,
-                (MaxCols==1&&MaxRows!=1) ? RowMajor : ColMajor,
-                MaxRows,
-                MaxCols
-          > type;
-};
-
-/** \internal The reference selector for template expressions. The idea is that we don't
-  * need to use references for expressions since they are light weight proxy
-  * objects which should generate no copying overhead. */
-template <typename T>
-struct ref_selector
-{
-  typedef typename conditional<
-    bool(traits<T>::Flags & NestByRefBit),
-    T const&,
-    const T
-  >::type type;
-  
-  typedef typename conditional<
-    bool(traits<T>::Flags & NestByRefBit),
-    T &,
-    T
-  >::type non_const_type;
-};
-
-/** \internal Adds the const qualifier on the value-type of T2 if and only if T1 is a const type */
-template<typename T1, typename T2>
-struct transfer_constness
-{
-  typedef typename conditional<
-    bool(internal::is_const<T1>::value),
-    typename internal::add_const_on_value_type<T2>::type,
-    T2
-  >::type type;
-};
-
-
-// However, we still need a mechanism to detect whether an expression which is evaluated multiple time
-// has to be evaluated into a temporary.
-// That's the purpose of this new nested_eval helper:
-/** \internal Determines how a given expression should be nested when evaluated multiple times.
-  * For example, when you do a * (b+c), Eigen will determine how the expression b+c should be
-  * evaluated into the bigger product expression. The choice is between nesting the expression b+c as-is, or
-  * evaluating that expression b+c into a temporary variable d, and nest d so that the resulting expression is
-  * a*d. Evaluating can be beneficial for example if every coefficient access in the resulting expression causes
-  * many coefficient accesses in the nested expressions -- as is the case with matrix product for example.
-  *
-  * \tparam T the type of the expression being nested.
-  * \tparam n the number of coefficient accesses in the nested expression for each coefficient access in the bigger expression.
-  * \tparam PlainObject the type of the temporary if needed.
-  */
-template<typename T, int n, typename PlainObject = typename plain_object_eval<T>::type> struct nested_eval
-{
-  enum {
-    ScalarReadCost = NumTraits<typename traits<T>::Scalar>::ReadCost,
-    CoeffReadCost = evaluator<T>::CoeffReadCost,  // NOTE What if an evaluator evaluate itself into a tempory?
-                                                  //      Then CoeffReadCost will be small (e.g., 1) but we still have to evaluate, especially if n>1.
-                                                  //      This situation is already taken care by the EvalBeforeNestingBit flag, which is turned ON
-                                                  //      for all evaluator creating a temporary. This flag is then propagated by the parent evaluators.
-                                                  //      Another solution could be to count the number of temps?
-    NAsInteger = n == Dynamic ? HugeCost : n,
-    CostEval   = (NAsInteger+1) * ScalarReadCost + CoeffReadCost,
-    CostNoEval = NAsInteger * CoeffReadCost,
-    Evaluate = (int(evaluator<T>::Flags) & EvalBeforeNestingBit) || (int(CostEval) < int(CostNoEval))
-  };
-
-  typedef typename conditional<Evaluate, PlainObject, typename ref_selector<T>::type>::type type;
-};
-
-template<typename T>
-EIGEN_DEVICE_FUNC
-inline T* const_cast_ptr(const T* ptr)
-{
-  return const_cast<T*>(ptr);
-}
-
-template<typename Derived, typename XprKind = typename traits<Derived>::XprKind>
-struct dense_xpr_base
-{
-  /* dense_xpr_base should only ever be used on dense expressions, thus falling either into the MatrixXpr or into the ArrayXpr cases */
-};
-
-template<typename Derived>
-struct dense_xpr_base<Derived, MatrixXpr>
-{
-  typedef MatrixBase<Derived> type;
-};
-
-template<typename Derived>
-struct dense_xpr_base<Derived, ArrayXpr>
-{
-  typedef ArrayBase<Derived> type;
-};
-
-template<typename Derived, typename XprKind = typename traits<Derived>::XprKind, typename StorageKind = typename traits<Derived>::StorageKind>
-struct generic_xpr_base;
-
-template<typename Derived, typename XprKind>
-struct generic_xpr_base<Derived, XprKind, Dense>
-{
-  typedef typename dense_xpr_base<Derived,XprKind>::type type;
-};
-
-template<typename XprType, typename CastType> struct cast_return_type
-{
-  typedef typename XprType::Scalar CurrentScalarType;
-  typedef typename remove_all<CastType>::type _CastType;
-  typedef typename _CastType::Scalar NewScalarType;
-  typedef typename conditional<is_same<CurrentScalarType,NewScalarType>::value,
-                              const XprType&,CastType>::type type;
-};
-
-template <typename A, typename B> struct promote_storage_type;
-
-template <typename A> struct promote_storage_type<A,A>
-{
-  typedef A ret;
-};
-template <typename A> struct promote_storage_type<A, const A>
-{
-  typedef A ret;
-};
-template <typename A> struct promote_storage_type<const A, A>
-{
-  typedef A ret;
-};
-
-/** \internal Specify the "storage kind" of applying a coefficient-wise
-  * binary operations between two expressions of kinds A and B respectively.
-  * The template parameter Functor permits to specialize the resulting storage kind wrt to
-  * the functor.
-  * The default rules are as follows:
-  * \code
-  * A      op A      -> A
-  * A      op dense  -> dense
-  * dense  op B      -> dense
-  * sparse op dense  -> sparse
-  * dense  op sparse -> sparse
-  * \endcode
-  */
-template <typename A, typename B, typename Functor> struct cwise_promote_storage_type;
-
-template <typename A, typename Functor>                   struct cwise_promote_storage_type<A,A,Functor>                                      { typedef A      ret; };
-template <typename Functor>                               struct cwise_promote_storage_type<Dense,Dense,Functor>                              { typedef Dense  ret; };
-template <typename A, typename Functor>                   struct cwise_promote_storage_type<A,Dense,Functor>                                  { typedef Dense  ret; };
-template <typename B, typename Functor>                   struct cwise_promote_storage_type<Dense,B,Functor>                                  { typedef Dense  ret; };
-template <typename Functor>                               struct cwise_promote_storage_type<Sparse,Dense,Functor>                             { typedef Sparse ret; };
-template <typename Functor>                               struct cwise_promote_storage_type<Dense,Sparse,Functor>                             { typedef Sparse ret; };
-
-template <typename LhsKind, typename RhsKind, int LhsOrder, int RhsOrder> struct cwise_promote_storage_order {
-  enum { value = LhsOrder };
-};
-
-template <typename LhsKind, int LhsOrder, int RhsOrder>   struct cwise_promote_storage_order<LhsKind,Sparse,LhsOrder,RhsOrder>                { enum { value = RhsOrder }; };
-template <typename RhsKind, int LhsOrder, int RhsOrder>   struct cwise_promote_storage_order<Sparse,RhsKind,LhsOrder,RhsOrder>                { enum { value = LhsOrder }; };
-template <int Order>                                      struct cwise_promote_storage_order<Sparse,Sparse,Order,Order>                       { enum { value = Order }; };
-
-
-/** \internal Specify the "storage kind" of multiplying an expression of kind A with kind B.
-  * The template parameter ProductTag permits to specialize the resulting storage kind wrt to
-  * some compile-time properties of the product: GemmProduct, GemvProduct, OuterProduct, InnerProduct.
-  * The default rules are as follows:
-  * \code
-  *  K * K            -> K
-  *  dense * K        -> dense
-  *  K * dense        -> dense
-  *  diag * K         -> K
-  *  K * diag         -> K
-  *  Perm * K         -> K
-  * K * Perm          -> K
-  * \endcode
-  */
-template <typename A, typename B, int ProductTag> struct product_promote_storage_type;
-
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  A,                  ProductTag> { typedef A     ret;};
-template <int ProductTag>             struct product_promote_storage_type<Dense,              Dense,              ProductTag> { typedef Dense ret;};
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  Dense,              ProductTag> { typedef Dense ret; };
-template <typename B, int ProductTag> struct product_promote_storage_type<Dense,              B,                  ProductTag> { typedef Dense ret; };
-
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  DiagonalShape,      ProductTag> { typedef A ret; };
-template <typename B, int ProductTag> struct product_promote_storage_type<DiagonalShape,      B,                  ProductTag> { typedef B ret; };
-template <int ProductTag>             struct product_promote_storage_type<Dense,              DiagonalShape,      ProductTag> { typedef Dense ret; };
-template <int ProductTag>             struct product_promote_storage_type<DiagonalShape,      Dense,              ProductTag> { typedef Dense ret; };
-
-template <typename A, int ProductTag> struct product_promote_storage_type<A,                  PermutationStorage, ProductTag> { typedef A ret; };
-template <typename B, int ProductTag> struct product_promote_storage_type<PermutationStorage, B,                  ProductTag> { typedef B ret; };
-template <int ProductTag>             struct product_promote_storage_type<Dense,              PermutationStorage, ProductTag> { typedef Dense ret; };
-template <int ProductTag>             struct product_promote_storage_type<PermutationStorage, Dense,              ProductTag> { typedef Dense ret; };
-
-/** \internal gives the plain matrix or array type to store a row/column/diagonal of a matrix type.
-  * \tparam Scalar optional parameter allowing to pass a different scalar type than the one of the MatrixType.
-  */
-template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
-struct plain_row_type
-{
-  typedef Matrix<Scalar, 1, ExpressionType::ColsAtCompileTime,
-                 ExpressionType::PlainObject::Options | RowMajor, 1, ExpressionType::MaxColsAtCompileTime> MatrixRowType;
-  typedef Array<Scalar, 1, ExpressionType::ColsAtCompileTime,
-                 ExpressionType::PlainObject::Options | RowMajor, 1, ExpressionType::MaxColsAtCompileTime> ArrayRowType;
-
-  typedef typename conditional<
-    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
-    MatrixRowType,
-    ArrayRowType 
-  >::type type;
-};
-
-template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
-struct plain_col_type
-{
-  typedef Matrix<Scalar, ExpressionType::RowsAtCompileTime, 1,
-                 ExpressionType::PlainObject::Options & ~RowMajor, ExpressionType::MaxRowsAtCompileTime, 1> MatrixColType;
-  typedef Array<Scalar, ExpressionType::RowsAtCompileTime, 1,
-                 ExpressionType::PlainObject::Options & ~RowMajor, ExpressionType::MaxRowsAtCompileTime, 1> ArrayColType;
-
-  typedef typename conditional<
-    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
-    MatrixColType,
-    ArrayColType 
-  >::type type;
-};
-
-template<typename ExpressionType, typename Scalar = typename ExpressionType::Scalar>
-struct plain_diag_type
-{
-  enum { diag_size = EIGEN_SIZE_MIN_PREFER_DYNAMIC(ExpressionType::RowsAtCompileTime, ExpressionType::ColsAtCompileTime),
-         max_diag_size = EIGEN_SIZE_MIN_PREFER_FIXED(ExpressionType::MaxRowsAtCompileTime, ExpressionType::MaxColsAtCompileTime)
-  };
-  typedef Matrix<Scalar, diag_size, 1, ExpressionType::PlainObject::Options & ~RowMajor, max_diag_size, 1> MatrixDiagType;
-  typedef Array<Scalar, diag_size, 1, ExpressionType::PlainObject::Options & ~RowMajor, max_diag_size, 1> ArrayDiagType;
-
-  typedef typename conditional<
-    is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
-    MatrixDiagType,
-    ArrayDiagType 
-  >::type type;
-};
-
-template<typename Expr,typename Scalar = typename Expr::Scalar>
-struct plain_constant_type
-{
-  enum { Options = (traits<Expr>::Flags&RowMajorBit)?RowMajor:0 };
-
-  typedef Array<Scalar,  traits<Expr>::RowsAtCompileTime,   traits<Expr>::ColsAtCompileTime,
-                Options, traits<Expr>::MaxRowsAtCompileTime,traits<Expr>::MaxColsAtCompileTime> array_type;
-
-  typedef Matrix<Scalar,  traits<Expr>::RowsAtCompileTime,   traits<Expr>::ColsAtCompileTime,
-                 Options, traits<Expr>::MaxRowsAtCompileTime,traits<Expr>::MaxColsAtCompileTime> matrix_type;
-
-  typedef CwiseNullaryOp<scalar_constant_op<Scalar>, const typename conditional<is_same< typename traits<Expr>::XprKind, MatrixXpr >::value, matrix_type, array_type>::type > type;
-};
-
-template<typename ExpressionType>
-struct is_lvalue
-{
-  enum { value = (!bool(is_const<ExpressionType>::value)) &&
-                 bool(traits<ExpressionType>::Flags & LvalueBit) };
-};
-
-template<typename T> struct is_diagonal
-{ enum { ret = false }; };
-
-template<typename T> struct is_diagonal<DiagonalBase<T> >
-{ enum { ret = true }; };
-
-template<typename T> struct is_diagonal<DiagonalWrapper<T> >
-{ enum { ret = true }; };
-
-template<typename T, int S> struct is_diagonal<DiagonalMatrix<T,S> >
-{ enum { ret = true }; };
-
-template<typename S1, typename S2> struct glue_shapes;
-template<> struct glue_shapes<DenseShape,TriangularShape> { typedef TriangularShape type;  };
-
-template<typename T1, typename T2>
-bool is_same_dense(const T1 &mat1, const T2 &mat2, typename enable_if<has_direct_access<T1>::ret&&has_direct_access<T2>::ret, T1>::type * = 0)
-{
-  return (mat1.data()==mat2.data()) && (mat1.innerStride()==mat2.innerStride()) && (mat1.outerStride()==mat2.outerStride());
-}
-
-template<typename T1, typename T2>
-bool is_same_dense(const T1 &, const T2 &, typename enable_if<!(has_direct_access<T1>::ret&&has_direct_access<T2>::ret), T1>::type * = 0)
-{
-  return false;
-}
-
-// Internal helper defining the cost of a scalar division for the type T.
-// The default heuristic can be specialized for each scalar type and architecture.
-template<typename T,bool Vectorized=false,typename EnaleIf = void>
-struct scalar_div_cost {
-  enum { value = 8*NumTraits<T>::MulCost };
-};
-
-template<typename T,bool Vectorized>
-struct scalar_div_cost<std::complex<T>, Vectorized> {
-  enum { value = 2*scalar_div_cost<T>::value
-               + 6*NumTraits<T>::MulCost
-               + 3*NumTraits<T>::AddCost
-  };
-};
-
-
-template<bool Vectorized>
-struct scalar_div_cost<signed long,Vectorized,typename conditional<sizeof(long)==8,void,false_type>::type> { enum { value = 24 }; };
-template<bool Vectorized>
-struct scalar_div_cost<unsigned long,Vectorized,typename conditional<sizeof(long)==8,void,false_type>::type> { enum { value = 21 }; };
-
-
-#ifdef EIGEN_DEBUG_ASSIGN
-std::string demangle_traversal(int t)
-{
-  if(t==DefaultTraversal) return "DefaultTraversal";
-  if(t==LinearTraversal) return "LinearTraversal";
-  if(t==InnerVectorizedTraversal) return "InnerVectorizedTraversal";
-  if(t==LinearVectorizedTraversal) return "LinearVectorizedTraversal";
-  if(t==SliceVectorizedTraversal) return "SliceVectorizedTraversal";
-  return "?";
-}
-std::string demangle_unrolling(int t)
-{
-  if(t==NoUnrolling) return "NoUnrolling";
-  if(t==InnerUnrolling) return "InnerUnrolling";
-  if(t==CompleteUnrolling) return "CompleteUnrolling";
-  return "?";
-}
-std::string demangle_flags(int f)
-{
-  std::string res;
-  if(f&RowMajorBit)                 res += " | RowMajor";
-  if(f&PacketAccessBit)             res += " | Packet";
-  if(f&LinearAccessBit)             res += " | Linear";
-  if(f&LvalueBit)                   res += " | Lvalue";
-  if(f&DirectAccessBit)             res += " | Direct";
-  if(f&NestByRefBit)                res += " | NestByRef";
-  if(f&NoPreferredStorageOrderBit)  res += " | NoPreferredStorageOrderBit";
-  
-  return res;
-}
-#endif
-
-} // end namespace internal
-
-
-/** \class ScalarBinaryOpTraits
-  * \ingroup Core_Module
-  *
-  * \brief Determines whether the given binary operation of two numeric types is allowed and what the scalar return type is.
-  *
-  * This class permits to control the scalar return type of any binary operation performed on two different scalar types through (partial) template specializations.
-  *
-  * For instance, let \c U1, \c U2 and \c U3 be three user defined scalar types for which most operations between instances of \c U1 and \c U2 returns an \c U3.
-  * You can let %Eigen knows that by defining:
-    \code
-    template<typename BinaryOp>
-    struct ScalarBinaryOpTraits<U1,U2,BinaryOp> { typedef U3 ReturnType;  };
-    template<typename BinaryOp>
-    struct ScalarBinaryOpTraits<U2,U1,BinaryOp> { typedef U3 ReturnType;  };
-    \endcode
-  * You can then explicitly disable some particular operations to get more explicit error messages:
-    \code
-    template<>
-    struct ScalarBinaryOpTraits<U1,U2,internal::scalar_max_op<U1,U2> > {};
-    \endcode
-  * Or customize the return type for individual operation:
-    \code
-    template<>
-    struct ScalarBinaryOpTraits<U1,U2,internal::scalar_sum_op<U1,U2> > { typedef U1 ReturnType; };
-    \endcode
-  *
-  * By default, the following generic combinations are supported:
-  <table class="manual">
-  <tr><th>ScalarA</th><th>ScalarB</th><th>BinaryOp</th><th>ReturnType</th><th>Note</th></tr>
-  <tr            ><td>\c T </td><td>\c T </td><td>\c * </td><td>\c T </td><td></td></tr>
-  <tr class="alt"><td>\c NumTraits<T>::Real </td><td>\c T </td><td>\c * </td><td>\c T </td><td>Only if \c NumTraits<T>::IsComplex </td></tr>
-  <tr            ><td>\c T </td><td>\c NumTraits<T>::Real </td><td>\c * </td><td>\c T </td><td>Only if \c NumTraits<T>::IsComplex </td></tr>
-  </table>
-  *
-  * \sa CwiseBinaryOp
-  */
-template<typename ScalarA, typename ScalarB, typename BinaryOp=internal::scalar_product_op<ScalarA,ScalarB> >
-struct ScalarBinaryOpTraits
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-  // for backward compatibility, use the hints given by the (deprecated) internal::scalar_product_traits class.
-  : internal::scalar_product_traits<ScalarA,ScalarB>
-#endif // EIGEN_PARSED_BY_DOXYGEN
-{};
-
-template<typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<T,T,BinaryOp>
-{
-  typedef T ReturnType;
-};
-
-template <typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<T, typename NumTraits<typename internal::enable_if<NumTraits<T>::IsComplex,T>::type>::Real, BinaryOp>
-{
-  typedef T ReturnType;
-};
-template <typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<typename NumTraits<typename internal::enable_if<NumTraits<T>::IsComplex,T>::type>::Real, T, BinaryOp>
-{
-  typedef T ReturnType;
-};
-
-// For Matrix * Permutation
-template<typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<T,void,BinaryOp>
-{
-  typedef T ReturnType;
-};
-
-// For Permutation * Matrix
-template<typename T, typename BinaryOp>
-struct ScalarBinaryOpTraits<void,T,BinaryOp>
-{
-  typedef T ReturnType;
-};
-
-// for Permutation*Permutation
-template<typename BinaryOp>
-struct ScalarBinaryOpTraits<void,void,BinaryOp>
-{
-  typedef void ReturnType;
-};
-
-// We require Lhs and Rhs to have "compatible" scalar types.
-// It is tempting to always allow mixing different types but remember that this is often impossible in the vectorized paths.
-// So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user tries to
-// add together a float matrix and a double matrix.
-#define EIGEN_CHECK_BINARY_COMPATIBILIY(BINOP,LHS,RHS) \
-  EIGEN_STATIC_ASSERT((Eigen::internal::has_ReturnType<ScalarBinaryOpTraits<LHS, RHS,BINOP> >::value), \
-    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
-    
-} // end namespace Eigen
-
-#endif // EIGEN_XPRHELPER_H

wpiutil/src/main/native/include/units/units.h

diff --git a/wpiutil/src/main/native/include/units/units.h b/wpiutil/src/main/native/include/units/units.h
deleted file mode 100644
index 4e4bdbd..0000000
--- a/wpiutil/src/main/native/include/units/units.h
+++ /dev/null
@@ -1,4857 +0,0 @@
-//--------------------------------------------------------------------------------------------------
-// 
-//	Units: A compile-time c++14 unit conversion library with no dependencies
-//
-//--------------------------------------------------------------------------------------------------
-//
-// The MIT License (MIT)
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
-// and associated documentation files (the "Software"), to deal in the Software without 
-// restriction, including without limitation the rights to use, copy, modify, merge, publish, 
-// distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the 
-// Software is furnished to do so, subject to the following conditions:
-// 
-// The above copyright notice and this permission notice shall be included in all copies or 
-// substantial portions of the Software.
-// 
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
-// BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
-// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
-// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
-// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
-//
-//--------------------------------------------------------------------------------------------------
-// 
-// Copyright (c) 2016 Nic Holthaus
-// 
-//--------------------------------------------------------------------------------------------------
-//
-// ATTRIBUTION:
-// Parts of this work have been adapted from: 
-// http://stackoverflow.com/questions/35069778/create-comparison-trait-for-template-classes-whose-parameters-are-in-a-different
-// http://stackoverflow.com/questions/28253399/check-traits-for-all-variadic-template-arguments/28253503
-// http://stackoverflow.com/questions/36321295/rational-approximation-of-square-root-of-stdratio-at-compile-time?noredirect=1#comment60266601_36321295
-//
-//--------------------------------------------------------------------------------------------------
-//
-/// @file	units.h
-/// @brief	Complete implementation of `units` - a compile-time, header-only, unit conversion 
-///			library built on c++14 with no dependencies.
-//
-//--------------------------------------------------------------------------------------------------
-
-#pragma once
-
-#ifndef units_h__
-#define units_h__
-
-#ifdef _MSC_VER
-#	pragma push_macro("pascal")
-#	undef pascal
-#	if _MSC_VER <= 1800
-#		define _ALLOW_KEYWORD_MACROS
-#		pragma warning(push)
-#		pragma warning(disable : 4520)
-#		pragma push_macro("constexpr")
-#		define constexpr /*constexpr*/
-#		pragma push_macro("noexcept")
-#		define noexcept throw()
-#	endif // _MSC_VER < 1800
-#endif // _MSC_VER
-
-#if !defined(_MSC_VER) || _MSC_VER > 1800
-#   define UNIT_HAS_LITERAL_SUPPORT
-#   define UNIT_HAS_VARIADIC_TEMPLATE_SUPPORT
-#endif
-
-#ifndef UNIT_LIB_DEFAULT_TYPE
-#   define UNIT_LIB_DEFAULT_TYPE double
-#endif
-
-//--------------------
-//	INCLUDES
-//--------------------
-
-#include <chrono>
-#include <ratio>
-#include <type_traits>
-#include <cstdint>
-#include <cmath>
-#include <limits>
-
-#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
-	#include <iostream>
-	#include <string>
-	#include <locale>
-
-	//------------------------------
-	//	STRING FORMATTER
-	//------------------------------
-
-	namespace units
-	{
-		namespace detail
-		{
-			template <typename T> std::string to_string(const T& t)
-			{
-				std::string str{ std::to_string(t) };
-				int offset{ 1 };
-
-				// remove trailing decimal points for integer value units. Locale aware!
-				struct lconv * lc;
-				lc = localeconv();
-				char decimalPoint = *lc->decimal_point;
-				if (str.find_last_not_of('0') == str.find(decimalPoint)) { offset = 0; }
-				str.erase(str.find_last_not_of('0') + offset, std::string::npos);
-				return str;
-			}
-		}
-	}
-#endif
-
-namespace units
-{
-	template<typename T> inline constexpr const char* name(const T&);
-	template<typename T> inline constexpr const char* abbreviation(const T&);
-}
-
-//------------------------------
-//	MACROS
-//------------------------------
-
-/**
- * @def			UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, definition)
- * @brief		Helper macro for generating the boiler-plate code generating the tags of a new unit.
- * @details		The macro generates singular, plural, and abbreviated forms
- *				of the unit definition (e.g. `meter`, `meters`, and `m`), as aliases for the
- *				unit tag.
- * @param		namespaceName namespace in which the new units will be encapsulated.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- * @param		namePlural - plural version of the unit name, e.g. 'meters'
- * @param		abbreviation - abbreviated unit name, e.g. 'm'
- * @param		definition - the variadic parameter is used for the definition of the unit
- *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
- * @note		a variadic template is used for the definition to allow templates with
- *				commas to be easily expanded. All the variadic 'arguments' should together
- *				comprise the unit definition.
- */
-#define UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, /*definition*/...)\
-	namespace namespaceName\
-	{\
-	/** @name Units (full names plural) */ /** @{ */ typedef __VA_ARGS__ namePlural; /** @} */\
-	/** @name Units (full names singular) */ /** @{ */ typedef namePlural nameSingular; /** @} */\
-	/** @name Units (abbreviated) */ /** @{ */ typedef namePlural abbreviation; /** @} */\
-	}
-
-/**
- * @def			UNIT_ADD_UNIT_DEFINITION(namespaceName,nameSingular)
- * @brief		Macro for generating the boiler-plate code for the unit_t type definition.
- * @details		The macro generates the definition of the unit container types, e.g. `meter_t`
- * @param		namespaceName namespace in which the new units will be encapsulated.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- */
-#define UNIT_ADD_UNIT_DEFINITION(namespaceName,nameSingular)\
-	namespace namespaceName\
-	{\
-		/** @name Unit Containers */ /** @{ */ typedef unit_t<nameSingular> nameSingular ## _t; /** @} */\
-	}
-
-/**
- * @def			UNIT_ADD_CUSTOM_TYPE_UNIT_DEFINITION(namespaceName,nameSingular,underlyingType)
- * @brief		Macro for generating the boiler-plate code for a unit_t type definition with a non-default underlying type.
- * @details		The macro generates the definition of the unit container types, e.g. `meter_t`
- * @param		namespaceName namespace in which the new units will be encapsulated.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- * @param		underlyingType the underlying type
- */
-#define UNIT_ADD_CUSTOM_TYPE_UNIT_DEFINITION(namespaceName,nameSingular, underlyingType)\
-	namespace namespaceName\
-	{\
-	/** @name Unit Containers */ /** @{ */ typedef unit_t<nameSingular,underlyingType> nameSingular ## _t; /** @} */\
-	}
-/**
- * @def			UNIT_ADD_IO(namespaceName,nameSingular, abbreviation)
- * @brief		Macro for generating the boiler-plate code needed for I/O for a new unit.
- * @details		The macro generates the code to insert units into an ostream. It
- *				prints both the value and abbreviation of the unit when invoked.
- * @param		namespaceName namespace in which the new units will be encapsulated.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- * @param		abbrev - abbreviated unit name, e.g. 'm'
- * @note		When UNIT_LIB_DISABLE_IOSTREAM is defined, the macro does not generate any code
- */
-#if defined(UNIT_LIB_DISABLE_IOSTREAM)
-	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)
-#else
-	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
-	namespace namespaceName\
-	{\
-		inline std::ostream& operator<<(std::ostream& os, const nameSingular ## _t& obj) \
-		{\
-			os << obj() << " "#abbrev; return os; \
-		}\
-		inline std::string to_string(const nameSingular ## _t& obj)\
-		{\
-			return units::detail::to_string(obj()) + std::string(" "#abbrev);\
-		}\
-	}
-#endif
-
- /**
-  * @def		UNIT_ADD_NAME(namespaceName,nameSingular,abbreviation)
-  * @brief		Macro for generating constexpr names/abbreviations for units.
-  * @details	The macro generates names for units. E.g. name() of 1_m would be "meter", and 
-  *				abbreviation would be "m".
-  * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
-  *				are placed in the `units::literals` namespace.
-  * @param		nameSingular singular version of the unit name, e.g. 'meter'
-  * @param		abbreviation - abbreviated unit name, e.g. 'm'
-  */
-#define UNIT_ADD_NAME(namespaceName, nameSingular, abbrev)\
-template<> inline constexpr const char* name(const namespaceName::nameSingular ## _t&)\
-{\
-	return #nameSingular;\
-}\
-template<> inline constexpr const char* abbreviation(const namespaceName::nameSingular ## _t&)\
-{\
-	return #abbrev;\
-}
-
-/**
- * @def			UNIT_ADD_LITERALS(namespaceName,nameSingular,abbreviation)
- * @brief		Macro for generating user-defined literals for units.
- * @details		The macro generates user-defined literals for units. A literal suffix is created
- *				using the abbreviation (e.g. `10.0_m`).
- * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
- *				are placed in the `units::literals` namespace.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- * @param		abbreviation - abbreviated unit name, e.g. 'm'
- * @note		When UNIT_HAS_LITERAL_SUPPORT is not defined, the macro does not generate any code
- */
-#if defined(UNIT_HAS_LITERAL_SUPPORT)
-	#define UNIT_ADD_LITERALS(namespaceName, nameSingular, abbreviation)\
-	namespace literals\
-	{\
-		inline constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation(long double d)\
-		{\
-			return namespaceName::nameSingular ## _t(static_cast<namespaceName::nameSingular ## _t::underlying_type>(d));\
-		}\
-		inline constexpr namespaceName::nameSingular ## _t operator""_ ## abbreviation (unsigned long long d)\
-		{\
-			return namespaceName::nameSingular ## _t(static_cast<namespaceName::nameSingular ## _t::underlying_type>(d));\
-		}\
-	}
-#else
-	#define UNIT_ADD_LITERALS(namespaceName, nameSingular, abbreviation)
-#endif
-
-/**
- * @def			UNIT_ADD(namespaceName,nameSingular, namePlural, abbreviation, definition)
- * @brief		Macro for generating the boiler-plate code needed for a new unit.
- * @details		The macro generates singular, plural, and abbreviated forms
- *				of the unit definition (e.g. `meter`, `meters`, and `m`), as well as the
- *				appropriately named unit container (e.g. `meter_t`). A literal suffix is created
- *				using the abbreviation (e.g. `10.0_m`). It also defines a class-specific
- *				cout function which prints both the value and abbreviation of the unit when invoked.
- * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
- *				are placed in the `units::literals` namespace.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- * @param		namePlural - plural version of the unit name, e.g. 'meters'
- * @param		abbreviation - abbreviated unit name, e.g. 'm'
- * @param		definition - the variadic parameter is used for the definition of the unit
- *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
- * @note		a variadic template is used for the definition to allow templates with
- *				commas to be easily expanded. All the variadic 'arguments' should together
- *				comprise the unit definition.
- */
-#define UNIT_ADD(namespaceName, nameSingular, namePlural, abbreviation, /*definition*/...)\
-	UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, __VA_ARGS__)\
-	UNIT_ADD_UNIT_DEFINITION(namespaceName,nameSingular)\
-	UNIT_ADD_NAME(namespaceName,nameSingular, abbreviation)\
-	UNIT_ADD_IO(namespaceName,nameSingular, abbreviation)\
-	UNIT_ADD_LITERALS(namespaceName,nameSingular, abbreviation)
-
-/**
- * @def			UNIT_ADD_WITH_CUSTOM_TYPE(namespaceName,nameSingular, namePlural, abbreviation, underlyingType, definition)
- * @brief		Macro for generating the boiler-plate code needed for a new unit with a non-default underlying type.
- * @details		The macro generates singular, plural, and abbreviated forms
- *				of the unit definition (e.g. `meter`, `meters`, and `m`), as well as the
- *				appropriately named unit container (e.g. `meter_t`). A literal suffix is created
- *				using the abbreviation (e.g. `10.0_m`). It also defines a class-specific
- *				cout function which prints both the value and abbreviation of the unit when invoked.
- * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
- *				are placed in the `units::literals` namespace.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- * @param		namePlural - plural version of the unit name, e.g. 'meters'
- * @param		abbreviation - abbreviated unit name, e.g. 'm'
- * @param		underlyingType - the underlying type, e.g. 'int' or 'float'
- * @param		definition - the variadic parameter is used for the definition of the unit
- *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
- * @note		a variadic template is used for the definition to allow templates with
- *				commas to be easily expanded. All the variadic 'arguments' should together
- *				comprise the unit definition.
- */
-#define UNIT_ADD_WITH_CUSTOM_TYPE(namespaceName, nameSingular, namePlural, abbreviation, underlyingType, /*definition*/...)\
-	UNIT_ADD_UNIT_TAGS(namespaceName,nameSingular, namePlural, abbreviation, __VA_ARGS__)\
-	UNIT_ADD_CUSTOM_TYPE_UNIT_DEFINITION(namespaceName,nameSingular,underlyingType)\
-	UNIT_ADD_IO(namespaceName,nameSingular, abbreviation)\
-	UNIT_ADD_LITERALS(namespaceName,nameSingular, abbreviation)
-
-/**
- * @def			UNIT_ADD_DECIBEL(namespaceName, nameSingular, abbreviation)
- * @brief		Macro to create decibel container and literals for an existing unit type.
- * @details		This macro generates the decibel unit container, cout overload, and literal definitions.
- * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
- *				are placed in the `units::literals` namespace.
- * @param		nameSingular singular version of the base unit name, e.g. 'watt'
- * @param		abbreviation - abbreviated decibel unit name, e.g. 'dBW'
- */
-#define UNIT_ADD_DECIBEL(namespaceName, nameSingular, abbreviation)\
-	namespace namespaceName\
-	{\
-		/** @name Unit Containers */ /** @{ */ typedef unit_t<nameSingular, UNIT_LIB_DEFAULT_TYPE, units::decibel_scale> abbreviation ## _t; /** @} */\
-	}\
-	UNIT_ADD_IO(namespaceName, abbreviation, abbreviation)\
-	UNIT_ADD_LITERALS(namespaceName, abbreviation, abbreviation)
-
-/**
- * @def			UNIT_ADD_CATEGORY_TRAIT(unitCategory, baseUnit)
- * @brief		Macro to create the `is_category_unit` type trait.
- * @details		This trait allows users to test whether a given type matches
- *				an intended category. This macro comprises all the boiler-plate
- *				code necessary to do so.
- * @param		unitCategory The name of the category of unit, e.g. length or mass.
- */
-
-#define UNIT_ADD_CATEGORY_TRAIT_DETAIL(unitCategory)\
-	namespace traits\
-	{\
-		/** @cond */\
-		namespace detail\
-		{\
-			template<typename T> struct is_ ## unitCategory ## _unit_impl : std::false_type {};\
-			template<typename C, typename U, typename P, typename T>\
-			struct is_ ## unitCategory ## _unit_impl<units::unit<C, U, P, T>> : std::is_same<units::traits::base_unit_of<typename units::traits::unit_traits<units::unit<C, U, P, T>>::base_unit_type>, units::category::unitCategory ## _unit>::type {};\
-			template<typename U, typename S, template<typename> class N>\
-			struct is_ ## unitCategory ## _unit_impl<units::unit_t<U, S, N>> : std::is_same<units::traits::base_unit_of<typename units::traits::unit_t_traits<units::unit_t<U, S, N>>::unit_type>, units::category::unitCategory ## _unit>::type {};\
-		}\
-		/** @endcond */\
-	}
-
-#if defined(UNIT_HAS_VARIADIC_TEMPLATE_SUPPORT)
-#define UNIT_ADD_IS_UNIT_CATEGORY_TRAIT(unitCategory)\
-	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> {};\
-	}
-#else
-#define UNIT_ADD_IS_UNIT_CATEGORY_TRAIT(unitCategory)\
-	namespace traits\
-	{\
-			template<typename T1, typename T2 = T1, typename T3 = T1>\
-			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>{};\
-	}
-#endif
-
-#define UNIT_ADD_CATEGORY_TRAIT(unitCategory)\
-	UNIT_ADD_CATEGORY_TRAIT_DETAIL(unitCategory)\
-    /** @ingroup	TypeTraits*/\
-	/** @brief		Trait which tests whether a type represents a unit of unitCategory*/\
-	/** @details	Inherits from `std::true_type` or `std::false_type`. Use `is_ ## unitCategory ## _unit<T>::value` to test the unit represents a unitCategory quantity.*/\
-	/** @tparam		T	one or more types to test*/\
-	UNIT_ADD_IS_UNIT_CATEGORY_TRAIT(unitCategory)
-
-/**
- * @def			UNIT_ADD_WITH_METRIC_PREFIXES(nameSingular, namePlural, abbreviation, definition)
- * @brief		Macro for generating the boiler-plate code needed for a new unit, including its metric
- *				prefixes from femto to peta.
- * @details		See UNIT_ADD. In addition to generating the unit definition and containers '(e.g. `meters` and 'meter_t',
- *				it also creates corresponding units with metric suffixes such as `millimeters`, and `millimeter_t`), as well as the
- *				literal suffixes (e.g. `10.0_mm`).
- * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
- *				are placed in the `units::literals` namespace.
- * @param		nameSingular singular version of the unit name, e.g. 'meter'
- * @param		namePlural - plural version of the unit name, e.g. 'meters'
- * @param		abbreviation - abbreviated unit name, e.g. 'm'
- * @param		definition - the variadic parameter is used for the definition of the unit
- *				(e.g. `unit<std::ratio<1>, units::category::length_unit>`)
- * @note		a variadic template is used for the definition to allow templates with
- *				commas to be easily expanded. All the variadic 'arguments' should together
- *				comprise the unit definition.
- */
-#define UNIT_ADD_WITH_METRIC_PREFIXES(namespaceName, nameSingular, namePlural, abbreviation, /*definition*/...)\
-	UNIT_ADD(namespaceName, nameSingular, namePlural, abbreviation, __VA_ARGS__)\
-	UNIT_ADD(namespaceName, femto ## nameSingular, femto ## namePlural, f ## abbreviation, femto<namePlural>)\
-	UNIT_ADD(namespaceName, pico ## nameSingular, pico ## namePlural, p ## abbreviation, pico<namePlural>)\
-	UNIT_ADD(namespaceName, nano ## nameSingular, nano ## namePlural, n ## abbreviation, nano<namePlural>)\
-	UNIT_ADD(namespaceName, micro ## nameSingular, micro ## namePlural, u ## abbreviation, micro<namePlural>)\
-	UNIT_ADD(namespaceName, milli ## nameSingular, milli ## namePlural, m ## abbreviation, milli<namePlural>)\
-	UNIT_ADD(namespaceName, centi ## nameSingular, centi ## namePlural, c ## abbreviation, centi<namePlural>)\
-	UNIT_ADD(namespaceName, deci ## nameSingular, deci ## namePlural, d ## abbreviation, deci<namePlural>)\
-	UNIT_ADD(namespaceName, deca ## nameSingular, deca ## namePlural, da ## abbreviation, deca<namePlural>)\
-	UNIT_ADD(namespaceName, hecto ## nameSingular, hecto ## namePlural, h ## abbreviation, hecto<namePlural>)\
-	UNIT_ADD(namespaceName, kilo ## nameSingular, kilo ## namePlural, k ## abbreviation, kilo<namePlural>)\
-	UNIT_ADD(namespaceName, mega ## nameSingular, mega ## namePlural, M ## abbreviation, mega<namePlural>)\
-	UNIT_ADD(namespaceName, giga ## nameSingular, giga ## namePlural, G ## abbreviation, giga<namePlural>)\
-	UNIT_ADD(namespaceName, tera ## nameSingular, tera ## namePlural, T ## abbreviation, tera<namePlural>)\
-	UNIT_ADD(namespaceName, peta ## nameSingular, peta ## namePlural, P ## abbreviation, peta<namePlural>)\
-
- /**
-  * @def		UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(nameSingular, namePlural, abbreviation, definition)
-  * @brief		Macro for generating the boiler-plate code needed for a new unit, including its metric
-  *				prefixes from femto to peta, and binary prefixes from kibi to exbi.
-  * @details	See UNIT_ADD. In addition to generating the unit definition and containers '(e.g. `bytes` and 'byte_t',
-  *				it also creates corresponding units with metric suffixes such as `millimeters`, and `millimeter_t`), as well as the
-  *				literal suffixes (e.g. `10.0_B`).
-  * @param		namespaceName namespace in which the new units will be encapsulated. All literal values
-  *				are placed in the `units::literals` namespace.
-  * @param		nameSingular singular version of the unit name, e.g. 'byte'
-  * @param		namePlural - plural version of the unit name, e.g. 'bytes'
-  * @param		abbreviation - abbreviated unit name, e.g. 'B'
-  * @param		definition - the variadic parameter is used for the definition of the unit
-  *				(e.g. `unit<std::ratio<1>, units::category::data_unit>`)
-  * @note		a variadic template is used for the definition to allow templates with
-  *				commas to be easily expanded. All the variadic 'arguments' should together
-  *				comprise the unit definition.
-  */
-#define UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(namespaceName, nameSingular, namePlural, abbreviation, /*definition*/...)\
-	UNIT_ADD_WITH_METRIC_PREFIXES(namespaceName, nameSingular, namePlural, abbreviation, __VA_ARGS__)\
-	UNIT_ADD(namespaceName, kibi ## nameSingular, kibi ## namePlural, Ki ## abbreviation, kibi<namePlural>)\
-	UNIT_ADD(namespaceName, mebi ## nameSingular, mebi ## namePlural, Mi ## abbreviation, mebi<namePlural>)\
-	UNIT_ADD(namespaceName, gibi ## nameSingular, gibi ## namePlural, Gi ## abbreviation, gibi<namePlural>)\
-	UNIT_ADD(namespaceName, tebi ## nameSingular, tebi ## namePlural, Ti ## abbreviation, tebi<namePlural>)\
-	UNIT_ADD(namespaceName, pebi ## nameSingular, pebi ## namePlural, Pi ## abbreviation, pebi<namePlural>)\
-	UNIT_ADD(namespaceName, exbi ## nameSingular, exbi ## namePlural, Ei ## abbreviation, exbi<namePlural>)
-
-//--------------------
-//	UNITS NAMESPACE
-//--------------------
-
-/**
- * @namespace units
- * @brief Unit Conversion Library namespace
- */
-namespace units
-{
-	//----------------------------------
-	//	DOXYGEN
-	//----------------------------------
-
-	/**
-	 * @defgroup	UnitContainers Unit Containers
-	 * @brief		Defines a series of classes which contain dimensioned values. Unit containers
-	 *				store a value, and support various arithmetic operations.
-	 */
-
-	/**
-	 * @defgroup	UnitTypes Unit Types
-	 * @brief		Defines a series of classes which represent units. These types are tags used by
-	 *				the conversion function, to create compound units, or to create `unit_t` types.
-	 *				By themselves, they are not containers and have no stored value.
-	 */
-
-	/**
-	 * @defgroup	UnitManipulators Unit Manipulators
-	 * @brief		Defines a series of classes used to manipulate unit types, such as `inverse<>`, `squared<>`, and metric prefixes. 
-	 *				Unit manipulators can be chained together, e.g. `inverse<squared<pico<time::seconds>>>` to
-	 *				represent picoseconds^-2.
-	 */
-
-	 /**
-	  * @defgroup	CompileTimeUnitManipulators Compile-time Unit Manipulators
-	  * @brief		Defines a series of classes used to manipulate `unit_value_t` types at compile-time, such as `unit_value_add<>`, `unit_value_sqrt<>`, etc.
-	  *				Compile-time manipulators can be chained together, e.g. `unit_value_sqrt<unit_value_add<unit_value_power<a, 2>, unit_value_power<b, 2>>>` to
-	  *				represent `c = sqrt(a^2 + b^2).
-	  */
-
-	 /**
-	 * @defgroup	UnitMath Unit Math
-	 * @brief		Defines a collection of unit-enabled, strongly-typed versions of `<cmath>` functions.
-	 * @details		Includes most c++11 extensions.
-	 */
-
-	/**
-	 * @defgroup	Conversion Explicit Conversion
-	 * @brief		Functions used to convert values of one logical type to another.
-	 */
-
-	/**
-	 * @defgroup	TypeTraits Type Traits
-	 * @brief		Defines a series of classes to obtain unit type information at compile-time.
-	 */
-
-	//------------------------------
-	//	FORWARD DECLARATIONS
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace constants
-	{
-		namespace detail
-		{
-			static constexpr const UNIT_LIB_DEFAULT_TYPE PI_VAL = 3.14159265358979323846264338327950288419716939937510;
-		}
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	//------------------------------
-	//	RATIO TRAITS
-	//------------------------------
-
-	/**
-	 * @ingroup TypeTraits
-	 * @{
-	 */
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/// has_num implementation.
-		template<class T>
-		struct has_num_impl
-		{
-			template<class U>
-			static constexpr auto test(U*)->std::is_integral<decltype(U::num)> {return std::is_integral<decltype(U::num)>{}; }
-			template<typename>
-			static constexpr std::false_type test(...) { return std::false_type{}; }
-
-			using type = decltype(test<T>(0));
-		};
-	}
-
-	/**
-	 * @brief		Trait which checks for the existence of a static numerator.
-	 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_num<T>::value` to test
-	 *				whether `class T` has a numerator static member.
-	 */
-	template<class T>
-	struct has_num : units::detail::has_num_impl<T>::type {};
-
-	namespace detail
-	{
-		/// has_den implementation.
-		template<class T>
-		struct has_den_impl
-		{
-			template<class U>
-			static constexpr auto test(U*)->std::is_integral<decltype(U::den)> { return std::is_integral<decltype(U::den)>{}; }
-			template<typename>
-			static constexpr std::false_type test(...) { return std::false_type{}; }
-
-			using type = decltype(test<T>(0));
-		};
-	}
-
-	/**
-	 * @brief		Trait which checks for the existence of a static denominator.
-	 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_den<T>::value` to test
-	 *				whether `class T` has a denominator static member.
-	 */
-	template<class T>
-	struct has_den : units::detail::has_den_impl<T>::type {};
-
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace traits
-	{
-		/**
-		 * @brief		Trait that tests whether a type represents a std::ratio.
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_ratio<T>::value` to test
-		 *				whether `class T` implements a std::ratio.
-		 */
-		template<class T>
-		struct is_ratio : std::integral_constant<bool,
-			has_num<T>::value &&
-			has_den<T>::value>
-		{};
-	}
-
-	//------------------------------
-	//	UNIT TRAITS
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	/**
-	 * @brief		void type.
-	 * @details		Helper class for creating type traits.
-	 */
-	template<class ...>
-	struct void_t { typedef void type; };
-
-	/**
-	 * @brief		parameter pack for boolean arguments.
-	 */
-	template<bool...> struct bool_pack {};
-
-	/**
-	 * @brief		Trait which tests that a set of other traits are all true.
-	 */
-	template<bool... Args>
-	struct all_true : std::is_same<units::bool_pack<true, Args...>, units::bool_pack<Args..., true>> {};
-	/** @endcond */	// DOXYGEN IGNORE
-	
-	/** 
-	 * @brief namespace representing type traits which can access the properties of types provided by the units library.
-	 */
-	namespace traits
-	{
-#ifdef FOR_DOXYGEN_PURPOSES_ONLY
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Traits class defining the properties of units.
-		 * @details		The units library determines certain properties of the units passed to 
-		 *				them and what they represent by using the members of the corresponding 
-		 *				unit_traits instantiation.
-		 */
-		template<class T>
-		struct unit_traits
-		{
-			typedef typename T::base_unit_type base_unit_type;											///< Unit type that the unit was derived from. May be a `base_unit` or another `unit`. Use the `base_unit_of` trait to find the SI base unit type. This will be `void` if type `T` is not a unit.
-			typedef typename T::conversion_ratio conversion_ratio;										///< `std::ratio` representing the conversion factor to the `base_unit_type`. This will be `void` if type `T` is not a unit.
-			typedef typename T::pi_exponent_ratio pi_exponent_ratio;									///< `std::ratio` representing the exponent of pi to be used in the conversion. This will be `void` if type `T` is not a unit.
-			typedef typename T::translation_ratio translation_ratio;									///< `std::ratio` representing a datum translation to the base unit (i.e. degrees C to degrees F conversion). This will be `void` if type `T` is not a unit.
-		};
-#endif
-		/** @cond */	// DOXYGEN IGNORE
-		/**
-		 * @brief		unit traits implementation for classes which are not units.
-		 */
-		template<class T, typename = void>
-		struct unit_traits
-		{
-			typedef void base_unit_type;
-			typedef void conversion_ratio;
-			typedef void pi_exponent_ratio;
-			typedef void translation_ratio;
-		};
-
-		template<class T>
-		struct unit_traits
-			<T, typename void_t<
-			typename T::base_unit_type,
-			typename T::conversion_ratio,
-			typename T::pi_exponent_ratio,
-			typename T::translation_ratio>::type>
-		{
-			typedef typename T::base_unit_type base_unit_type;											///< Unit type that the unit was derived from. May be a `base_unit` or another `unit`. Use the `base_unit_of` trait to find the SI base unit type. This will be `void` if type `T` is not a unit.
-			typedef typename T::conversion_ratio conversion_ratio;										///< `std::ratio` representing the conversion factor to the `base_unit_type`. This will be `void` if type `T` is not a unit.
-			typedef typename T::pi_exponent_ratio pi_exponent_ratio;									///< `std::ratio` representing the exponent of pi to be used in the conversion. This will be `void` if type `T` is not a unit.
-			typedef typename T::translation_ratio translation_ratio;									///< `std::ratio` representing a datum translation to the base unit (i.e. degrees C to degrees F conversion). This will be `void` if type `T` is not a unit.
-		};
-		/** @endcond */	// END DOXYGEN IGNORE
-	}
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		helper type to identify base units.
-		 * @details		A non-templated base class for `base_unit` which enables RTTI testing.
-		 */
-		struct _base_unit_t {};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace traits
-	{
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests if a class is a `base_unit` type.
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_base_unit<T>::value` to test
-		 *				whether `class T` implements a `base_unit`.
-		 */
-		template<class T>
-		struct is_base_unit : std::is_base_of<units::detail::_base_unit_t, T> {};
-	}
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		helper type to identify units.
-		 * @details		A non-templated base class for `unit` which enables RTTI testing.
-		 */
-		struct _unit {};
-
-		template<std::intmax_t Num, std::intmax_t Den = 1>
-		using meter_ratio = std::ratio<Num, Den>;
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace traits
-	{
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Traits which tests if a class is a `unit`
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_unit<T>::value` to test
-		 *				whether `class T` implements a `unit`.
-		 */
-		template<class T>
-		struct is_unit : std::is_base_of<units::detail::_unit, T>::type {};
-	}
-
-	/** @} */ // end of TypeTraits
-
-	//------------------------------
-	//	BASE UNIT CLASS
-	//------------------------------
-
-	/**
-	 * @ingroup		UnitTypes
-	 * @brief		Class representing SI base unit types.
-	 * @details		Base units are represented by a combination of `std::ratio` template parameters, each
-	 *				describing the exponent of the type of unit they represent. Example: meters per second
-	 *				would be described by a +1 exponent for meters, and a -1 exponent for seconds, thus:
-	 *				`base_unit<std::ratio<1>, std::ratio<0>, std::ratio<-1>>`
-	 * @tparam		Meter		`std::ratio` representing the exponent value for meters.
-	 * @tparam		Kilogram	`std::ratio` representing the exponent value for kilograms.
-	 * @tparam		Second		`std::ratio` representing the exponent value for seconds.
-	 * @tparam		Radian		`std::ratio` representing the exponent value for radians. Although radians are not SI base units, they are included because radians are described by the SI as m * m^-1, which would make them indistinguishable from scalars.
-	 * @tparam		Ampere		`std::ratio` representing the exponent value for amperes.
-	 * @tparam		Kelvin		`std::ratio` representing the exponent value for Kelvin.
-	 * @tparam		Mole		`std::ratio` representing the exponent value for moles.
-	 * @tparam		Candela		`std::ratio` representing the exponent value for candelas.
-	 * @tparam		Byte		`std::ratio` representing the exponent value for bytes.
-	 * @sa			category	 for type aliases for SI base_unit types.
-	 */
-	template<class Meter = detail::meter_ratio<0>,
-	class Kilogram = std::ratio<0>,
-	class Second = std::ratio<0>,
-	class Radian = std::ratio<0>,
-	class Ampere = std::ratio<0>,
-	class Kelvin = std::ratio<0>,
-	class Mole = std::ratio<0>,
-	class Candela = std::ratio<0>,
-	class Byte = std::ratio<0>>
-	struct base_unit : units::detail::_base_unit_t
-	{
-		static_assert(traits::is_ratio<Meter>::value, "Template parameter `Meter` must be a `std::ratio` representing the exponent of meters the unit has");
-		static_assert(traits::is_ratio<Kilogram>::value, "Template parameter `Kilogram` must be a `std::ratio` representing the exponent of kilograms the unit has");
-		static_assert(traits::is_ratio<Second>::value, "Template parameter `Second` must be a `std::ratio` representing the exponent of seconds the unit has");
-		static_assert(traits::is_ratio<Ampere>::value, "Template parameter `Ampere` must be a `std::ratio` representing the exponent of amperes the unit has");
-		static_assert(traits::is_ratio<Kelvin>::value, "Template parameter `Kelvin` must be a `std::ratio` representing the exponent of kelvin the unit has");
-		static_assert(traits::is_ratio<Candela>::value, "Template parameter `Candela` must be a `std::ratio` representing the exponent of candelas the unit has");
-		static_assert(traits::is_ratio<Mole>::value, "Template parameter `Mole` must be a `std::ratio` representing the exponent of moles the unit has");
-		static_assert(traits::is_ratio<Radian>::value, "Template parameter `Radian` must be a `std::ratio` representing the exponent of radians the unit has");
-		static_assert(traits::is_ratio<Byte>::value, "Template parameter `Byte` must be a `std::ratio` representing the exponent of bytes the unit has");
-
-		typedef Meter meter_ratio;
-		typedef Kilogram kilogram_ratio;
-		typedef Second second_ratio;
-		typedef Radian radian_ratio;
-		typedef Ampere ampere_ratio;
-		typedef Kelvin kelvin_ratio;
-		typedef Mole mole_ratio;
-		typedef Candela candela_ratio;
-		typedef Byte byte_ratio;
-	};
-
-	//------------------------------
-	//	UNIT CATEGORIES
-	//------------------------------
-
-	/**
-	 * @brief		namespace representing the implemented base and derived unit types. These will not generally be needed by library users.
-	 * @sa			base_unit for the definition of the category parameters.
-	 */
-	namespace category
-	{
-		// SCALAR (DIMENSIONLESS) TYPES
-		typedef base_unit<> scalar_unit;			///< Represents a quantity with no dimension.
-		typedef base_unit<> dimensionless_unit;	///< Represents a quantity with no dimension.
-
-		// SI BASE UNIT TYPES
-		//					METERS			KILOGRAMS		SECONDS			RADIANS			AMPERES			KELVIN			MOLE			CANDELA			BYTE		---		CATEGORY
-		typedef base_unit<detail::meter_ratio<1>>																																		length_unit;			 		///< Represents an SI base unit of length
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<1>>																														mass_unit;				 		///< Represents an SI base unit of mass
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<1>>																										time_unit;				 		///< Represents an SI base unit of time
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>																						angle_unit;				 		///< Represents an SI base unit of angle
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>																		current_unit;			 		///< Represents an SI base unit of current
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>														temperature_unit;		 		///< Represents an SI base unit of temperature
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>										substance_unit;			 		///< Represents an SI base unit of amount of substance
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>						luminous_intensity_unit; 		///< Represents an SI base unit of luminous intensity
-
-		// SI DERIVED UNIT TYPES
-		//					METERS			KILOGRAMS		SECONDS			RADIANS			AMPERES			KELVIN			MOLE			CANDELA			BYTE		---		CATEGORY	
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>>						solid_angle_unit;				///< Represents an SI derived unit of solid angle
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>>																										frequency_unit;					///< Represents an SI derived unit of frequency
-		typedef base_unit<detail::meter_ratio<1>,	std::ratio<0>,	std::ratio<-1>>																										velocity_unit;					///< Represents an SI derived unit of velocity
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>,	std::ratio<1>>																						angular_velocity_unit;			///< Represents an SI derived unit of angular velocity
-		typedef base_unit<detail::meter_ratio<1>,	std::ratio<0>,	std::ratio<-2>>																										acceleration_unit;				///< Represents an SI derived unit of acceleration
-		typedef base_unit<detail::meter_ratio<1>,	std::ratio<1>,	std::ratio<-2>>																										force_unit;						///< Represents an SI derived unit of force
-		typedef base_unit<detail::meter_ratio<-1>,	std::ratio<1>,	std::ratio<-2>>																										pressure_unit;					///< Represents an SI derived unit of pressure
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<1>,	std::ratio<0>,	std::ratio<1>>																		charge_unit;					///< Represents an SI derived unit of charge
-		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>>																										energy_unit;					///< Represents an SI derived unit of energy
-		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-3>>																										power_unit;						///< Represents an SI derived unit of power
-		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-3>,	std::ratio<0>,	std::ratio<-1>>																		voltage_unit;					///< Represents an SI derived unit of voltage
-		typedef base_unit<detail::meter_ratio<-2>,	std::ratio<-1>,	std::ratio<4>,	std::ratio<0>,	std::ratio<2>>																		capacitance_unit;				///< Represents an SI derived unit of capacitance
-		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-3>,	std::ratio<0>,	std::ratio<-2>>																		impedance_unit;					///< Represents an SI derived unit of impedance
-		typedef base_unit<detail::meter_ratio<-2>,	std::ratio<-1>,	std::ratio<3>,	std::ratio<0>,	std::ratio<2>>																		conductance_unit;				///< Represents an SI derived unit of conductance
-		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>,	std::ratio<0>,	std::ratio<-1>>																		magnetic_flux_unit;				///< Represents an SI derived unit of magnetic flux
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<1>,	std::ratio<-2>,	std::ratio<0>,	std::ratio<-1>>																		magnetic_field_strength_unit;	///< Represents an SI derived unit of magnetic field strength
-		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>,	std::ratio<0>,	std::ratio<-2>>																		inductance_unit;				///< Represents an SI derived unit of inductance
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>						luminous_flux_unit;				///< Represents an SI derived unit of luminous flux
-		typedef base_unit<detail::meter_ratio<-2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<2>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>						illuminance_unit;				///< Represents an SI derived unit of illuminance
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>>																										radioactivity_unit;				///< Represents an SI derived unit of radioactivity
-
-		// OTHER UNIT TYPES
-		//					METERS			KILOGRAMS		SECONDS			RADIANS			AMPERES			KELVIN			MOLE			CANDELA			BYTE		---		CATEGORY			
-		typedef base_unit<detail::meter_ratio<2>,	std::ratio<1>,	std::ratio<-2>>																										torque_unit;					///< Represents an SI derived unit of torque
-		typedef base_unit<detail::meter_ratio<2>>																																		area_unit;						///< Represents an SI derived unit of area
-		typedef base_unit<detail::meter_ratio<3>>																																		volume_unit;					///< Represents an SI derived unit of volume
-		typedef base_unit<detail::meter_ratio<-3>,	std::ratio<1>>																														density_unit;					///< Represents an SI derived unit of density
-		typedef base_unit<>																																						concentration_unit;				///< Represents a unit of concentration
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>		data_unit;						///< Represents a unit of data size
-		typedef base_unit<detail::meter_ratio<0>,	std::ratio<0>,	std::ratio<-1>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<0>,	std::ratio<1>>		data_transfer_rate_unit;		///< Represents a unit of data transfer rate
-	}
-
-	//------------------------------
-	//	UNIT CLASSES
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	/**
-	 * @brief		unit type template specialization for units derived from base units.
-	 */
-	template <class, class, class, class> struct unit;
-	template<class Conversion, class... Exponents, class PiExponent, class Translation>
-	struct unit<Conversion, base_unit<Exponents...>, PiExponent, Translation> : units::detail::_unit
-	{
-		static_assert(traits::is_ratio<Conversion>::value, "Template parameter `Conversion` must be a `std::ratio` representing the conversion factor to `BaseUnit`.");
-		static_assert(traits::is_ratio<PiExponent>::value, "Template parameter `PiExponent` must be a `std::ratio` representing the exponents of Pi the unit has.");
-		static_assert(traits::is_ratio<Translation>::value, "Template parameter `Translation` must be a `std::ratio` representing an additive translation required by the unit conversion.");
-
-		typedef typename units::base_unit<Exponents...> base_unit_type;
-		typedef Conversion conversion_ratio;
-		typedef Translation translation_ratio;
-		typedef PiExponent pi_exponent_ratio;
-	};
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @brief		Type representing an arbitrary unit.
-	 * @ingroup		UnitTypes
-	 * @details		`unit` types are used as tags for the `conversion` function. They are *not* containers
-	 *				(see `unit_t` for a  container class). Each unit is defined by:
-	 *
-	 *				- A `std::ratio` defining the conversion factor to the base unit type. (e.g. `std::ratio<1,12>` for inches to feet)
-	 *				- A base unit that the unit is derived from (or a unit category. Must be of type `unit` or `base_unit`)
-	 *				- An exponent representing factors of PI required by the conversion. (e.g. `std::ratio<-1>` for a radians to degrees conversion)
-	 *				- a ratio representing a datum translation required for the conversion (e.g. `std::ratio<32>` for a farenheit to celsius conversion)
-	 *
-	 *				Typically, a specific unit, like `meters`, would be implemented as a type alias
-	 *				of `unit`, i.e. `using meters = unit<std::ratio<1>, units::category::length_unit`, or
-	 *				`using inches = unit<std::ratio<1,12>, feet>`.
-	 * @tparam		Conversion	std::ratio representing scalar multiplication factor.
-	 * @tparam		BaseUnit	Unit type which this unit is derived from. May be a `base_unit`, or another `unit`.
-	 * @tparam		PiExponent	std::ratio representing the exponent of pi required by the conversion.
-	 * @tparam		Translation	std::ratio representing any datum translation required by the conversion.
-	 */
-	template<class Conversion, class BaseUnit, class PiExponent = std::ratio<0>, class Translation = std::ratio<0>>
-	struct unit : units::detail::_unit
-	{
-		static_assert(traits::is_unit<BaseUnit>::value, "Template parameter `BaseUnit` must be a `unit` type.");
-		static_assert(traits::is_ratio<Conversion>::value, "Template parameter `Conversion` must be a `std::ratio` representing the conversion factor to `BaseUnit`.");
-		static_assert(traits::is_ratio<PiExponent>::value, "Template parameter `PiExponent` must be a `std::ratio` representing the exponents of Pi the unit has.");
-
-		typedef typename units::traits::unit_traits<BaseUnit>::base_unit_type base_unit_type;
-		typedef typename std::ratio_multiply<typename BaseUnit::conversion_ratio, Conversion> conversion_ratio;
-		typedef typename std::ratio_add<typename BaseUnit::pi_exponent_ratio, PiExponent> pi_exponent_ratio;
-		typedef typename std::ratio_add<std::ratio_multiply<typename BaseUnit::conversion_ratio, Translation>, typename BaseUnit::translation_ratio> translation_ratio;
-	};
-
-	//------------------------------
-	//	BASE UNIT MANIPULATORS
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		base_unit_of trait implementation
-		 * @details		recursively seeks base_unit type that a unit is derived from. Since units can be
-		 *				derived from other units, the `base_unit_type` typedef may not represent this value.
-		 */
-		template<class> struct base_unit_of_impl;
-		template<class Conversion, class BaseUnit, class PiExponent, class Translation>
-		struct base_unit_of_impl<unit<Conversion, BaseUnit, PiExponent, Translation>> : base_unit_of_impl<BaseUnit> {};
-		template<class... Exponents>
-		struct base_unit_of_impl<base_unit<Exponents...>>
-		{
-			typedef base_unit<Exponents...> type;
-		};
-		template<>
-		struct base_unit_of_impl<void>
-		{
-			typedef void type;
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace traits
-	{
-		/**
-		 * @brief		Trait which returns the `base_unit` type that a unit is originally derived from.
-		 * @details		Since units can be derived from other `unit` types in addition to `base_unit` types,
-		 *				the `base_unit_type` typedef will not always be a `base_unit` (or unit category).
-		 *				Since compatible
-		 */
-		template<class U>
-		using base_unit_of = typename units::detail::base_unit_of_impl<U>::type;
-	}
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		implementation of base_unit_multiply
-		 * @details		'multiples' (adds exponent ratios of) two base unit types. Base units can be found
-		 *				using `base_unit_of`.
-		 */
-		template<class, class> struct base_unit_multiply_impl;
-		template<class... Exponents1, class... Exponents2>
-		struct base_unit_multiply_impl<base_unit<Exponents1...>, base_unit<Exponents2...>> {
-			using type = base_unit<std::ratio_add<Exponents1, Exponents2>...>;
-		};
-
-		/**
-		 * @brief		represents type of two base units multiplied together
-		 */
-		template<class U1, class U2>
-		using base_unit_multiply = typename base_unit_multiply_impl<U1, U2>::type;
-
-		/**
-		 * @brief		implementation of base_unit_divide
-		 * @details		'dived' (subtracts exponent ratios of) two base unit types. Base units can be found
-		 *				using `base_unit_of`.
-		 */
-		template<class, class> struct base_unit_divide_impl;
-		template<class... Exponents1, class... Exponents2>
-		struct base_unit_divide_impl<base_unit<Exponents1...>, base_unit<Exponents2...>> {
-			using type = base_unit<std::ratio_subtract<Exponents1, Exponents2>...>;
-		};
-
-		/**
-		 * @brief		represents the resulting type of `base_unit` U1 divided by U2.
-		 */
-		template<class U1, class U2>
-		using base_unit_divide = typename base_unit_divide_impl<U1, U2>::type;
-
-		/**
-		 * @brief		implementation of inverse_base
-		 * @details		multiplies all `base_unit` exponent ratios by -1. The resulting type represents
-		 *				the inverse base unit of the given `base_unit` type.
-		 */
-		template<class> struct inverse_base_impl;
-
-		template<class... Exponents>
-		struct inverse_base_impl<base_unit<Exponents...>> {
-			using type = base_unit<std::ratio_multiply<Exponents, std::ratio<-1>>...>;
-		};
-
-		/**
-		 * @brief		represent the inverse type of `class U`
-		 * @details		E.g. if `U` is `length_unit`, then `inverse<U>` will represent `length_unit^-1`.
-		 */
-		template<class U> using inverse_base = typename inverse_base_impl<U>::type;
-
-		/**
-		 * @brief		implementation of `squared_base`
-		 * @details		multiplies all the exponent ratios of the given class by 2. The resulting type is
-		 *				equivalent to the given type squared.
-		 */
-		template<class U> struct squared_base_impl;
-		template<class... Exponents>
-		struct squared_base_impl<base_unit<Exponents...>> {
-			using type = base_unit<std::ratio_multiply<Exponents, std::ratio<2>>...>;
-		};
-
-		/**
-		 * @brief		represents the type of a `base_unit` squared.
-		 * @details		E.g. `squared<length_unit>` will represent `length_unit^2`.
-		 */
-		template<class U> using squared_base = typename squared_base_impl<U>::type;
-
-		/**
-		 * @brief		implementation of `cubed_base`
-		 * @details		multiplies all the exponent ratios of the given class by 3. The resulting type is
-		 *				equivalent to the given type cubed.
-		 */
-		template<class U> struct cubed_base_impl;
-		template<class... Exponents>
-		struct cubed_base_impl<base_unit<Exponents...>> {
-			using type = base_unit<std::ratio_multiply<Exponents, std::ratio<3>>...>;
-		};
-
-		/**
-		 * @brief		represents the type of a `base_unit` cubed.
-		 * @details		E.g. `cubed<length_unit>` will represent `length_unit^3`.
-		 */
-		template<class U> using cubed_base = typename cubed_base_impl<U>::type;
-
-		/**
-		 * @brief		implementation of `sqrt_base`
-		 * @details		divides all the exponent ratios of the given class by 2. The resulting type is
-		 *				equivalent to the square root of the given type.
-		 */
-		template<class U> struct sqrt_base_impl;
-		template<class... Exponents>
-		struct sqrt_base_impl<base_unit<Exponents...>> {
-			using type = base_unit<std::ratio_divide<Exponents, std::ratio<2>>...>;
-		};
-
-		/**
-		 * @brief		represents the square-root type of a `base_unit`.
-		 * @details		E.g. `sqrt<length_unit>` will represent `length_unit^(1/2)`.
-		 */
-		template<class U> using sqrt_base = typename sqrt_base_impl<U>::type;
-
-		/**
-		 * @brief		implementation of `cbrt_base`
-		 * @details		divides all the exponent ratios of the given class by 3. The resulting type is
-		 *				equivalent to the given type's cube-root.
-		 */
-		template<class U> struct cbrt_base_impl;
-		template<class... Exponents>
-		struct cbrt_base_impl<base_unit<Exponents...>> {
-			using type = base_unit<std::ratio_divide<Exponents, std::ratio<3>>...>;
-		};
-
-		/**
-		 * @brief		represents the cube-root type of a `base_unit` .
-		 * @details		E.g. `cbrt<length_unit>` will represent `length_unit^(1/3)`.
-		 */
-		template<class U> using cbrt_base = typename cbrt_base_impl<U>::type;
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	//------------------------------
-	//	UNIT MANIPULATORS
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		implementation of `unit_multiply`.
-		 * @details		multiplies two units. The base unit becomes the base units of each with their exponents
-		 *				added together. The conversion factors of each are multiplied by each other. Pi exponent ratios
-		 *				are added, and datum translations are removed.
-		 */
-		template<class Unit1, class Unit2>
-		struct unit_multiply_impl
-		{
-			using type = unit < std::ratio_multiply<typename Unit1::conversion_ratio, typename Unit2::conversion_ratio>,
-				base_unit_multiply <traits::base_unit_of<typename Unit1::base_unit_type>, traits::base_unit_of<typename Unit2::base_unit_type>>,
-				std::ratio_add<typename Unit1::pi_exponent_ratio, typename Unit2::pi_exponent_ratio>,
-				std::ratio < 0 >> ;
-		};
-
-		/**
-		 * @brief		represents the type of two units multiplied together.
-		 * @details		recalculates conversion and exponent ratios at compile-time.
-		 */
-		template<class U1, class U2>
-		using unit_multiply = typename unit_multiply_impl<U1, U2>::type;
-
-		/**
-		 * @brief		implementation of `unit_divide`.
-		 * @details		divides two units. The base unit becomes the base units of each with their exponents
-		 *				subtracted from each other. The conversion factors of each are divided by each other. Pi exponent ratios
-		 *				are subtracted, and datum translations are removed.
-		 */
-		template<class Unit1, class Unit2>
-		struct unit_divide_impl
-		{
-			using type = unit < std::ratio_divide<typename Unit1::conversion_ratio, typename Unit2::conversion_ratio>,
-				base_unit_divide<traits::base_unit_of<typename Unit1::base_unit_type>, traits::base_unit_of<typename Unit2::base_unit_type>>,
-				std::ratio_subtract<typename Unit1::pi_exponent_ratio, typename Unit2::pi_exponent_ratio>,
-				std::ratio < 0 >> ;
-		};
-
-		/**
-		 * @brief		represents the type of two units divided by each other.
-		 * @details		recalculates conversion and exponent ratios at compile-time.
-		 */
-		template<class U1, class U2>
-		using unit_divide = typename unit_divide_impl<U1, U2>::type;
-
-		/**
-		 * @brief		implementation of `inverse`
-		 * @details		inverts a unit (equivalent to 1/unit). The `base_unit` and pi exponents are all multiplied by
-		 *				-1. The conversion ratio numerator and denominator are swapped. Datum translation
-		 *				ratios are removed.
-		 */
-		template<class Unit>
-		struct inverse_impl
-		{
-			using type = unit < std::ratio<Unit::conversion_ratio::den, Unit::conversion_ratio::num>,
-				inverse_base<traits::base_unit_of<typename units::traits::unit_traits<Unit>::base_unit_type>>,
-				std::ratio_multiply<typename units::traits::unit_traits<Unit>::pi_exponent_ratio, std::ratio<-1>>,
-				std::ratio < 0 >> ;	// inverses are rates or change, the translation factor goes away.
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @brief		represents the inverse unit type of `class U`.
-	 * @ingroup		UnitManipulators
-	 * @tparam		U	`unit` type to invert.
-	 * @details		E.g. `inverse<meters>` will represent meters^-1 (i.e. 1/meters).
-	 */
-	template<class U> using inverse = typename units::detail::inverse_impl<U>::type;
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		implementation of `squared`
-		 * @details		Squares the conversion ratio, `base_unit` exponents, pi exponents, and removes
-		 *				datum translation ratios.
-		 */
-		template<class Unit>
-		struct squared_impl
-		{
-			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
-			using Conversion = typename Unit::conversion_ratio;
-			using type = unit < std::ratio_multiply<Conversion, Conversion>,
-				squared_base<traits::base_unit_of<typename Unit::base_unit_type>>,
-				std::ratio_multiply<typename Unit::pi_exponent_ratio, std::ratio<2>>,
-				typename Unit::translation_ratio
-			> ;
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @brief		represents the unit type of `class U` squared
-	 * @ingroup		UnitManipulators
-	 * @tparam		U	`unit` type to square.
-	 * @details		E.g. `square<meters>` will represent meters^2.
-	 */
-	template<class U>
-	using squared = typename units::detail::squared_impl<U>::type;
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-			 * @brief		implementation of `cubed`
-			 * @details		Cubes the conversion ratio, `base_unit` exponents, pi exponents, and removes
-			 *				datum translation ratios.
-			 */
-		template<class Unit>
-		struct cubed_impl
-		{
-			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
-			using Conversion = typename Unit::conversion_ratio;
-			using type = unit < std::ratio_multiply<Conversion, std::ratio_multiply<Conversion, Conversion>>,
-				cubed_base<traits::base_unit_of<typename Unit::base_unit_type>>,
-				std::ratio_multiply<typename Unit::pi_exponent_ratio, std::ratio<3>>,
-				typename Unit::translation_ratio> ;
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @brief		represents the type of `class U` cubed.
-	 * @ingroup		UnitManipulators
-	 * @tparam		U	`unit` type to cube.
-	 * @details		E.g. `cubed<meters>` will represent meters^3.
-	 */
-	template<class U>
-	using cubed = typename units::detail::cubed_impl<U>::type;
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		//----------------------------------
-		//	RATIO_SQRT IMPLEMENTATION
-		//----------------------------------
-
-		using Zero = std::ratio<0>;
-		using One = std::ratio<1>;
-		template <typename R> using Square = std::ratio_multiply<R, R>;
-
-		// Find the largest std::integer N such that Predicate<N>::value is true.
-		template <template <std::intmax_t N> class Predicate, typename enabled = void>
-		struct BinarySearch {
-			template <std::intmax_t N>
-			struct SafeDouble_ {
-				static constexpr const std::intmax_t value = 2 * N;
-				static_assert(value > 0, "Overflows when computing 2 * N");
-			};
-
-			template <intmax_t Lower, intmax_t Upper, typename Condition1 = void, typename Condition2 = void>
-			struct DoubleSidedSearch_ : DoubleSidedSearch_<Lower, Upper,
-				std::integral_constant<bool, (Upper - Lower == 1)>,
-				std::integral_constant<bool, ((Upper - Lower>1 && Predicate<Lower + (Upper - Lower) / 2>::value))>> {};
-
-			template <intmax_t Lower, intmax_t Upper>
-			struct DoubleSidedSearch_<Lower, Upper, std::false_type, std::false_type> : DoubleSidedSearch_<Lower, Lower + (Upper - Lower) / 2> {};
-
-			template <intmax_t Lower, intmax_t Upper, typename Condition2>
-			struct DoubleSidedSearch_<Lower, Upper, std::true_type, Condition2> : std::integral_constant<intmax_t, Lower>{};
-
-			template <intmax_t Lower, intmax_t Upper, typename Condition1>
-			struct DoubleSidedSearch_<Lower, Upper, Condition1, std::true_type> : DoubleSidedSearch_<Lower + (Upper - Lower) / 2, Upper>{};
-
-			template <std::intmax_t Lower, class enabled1 = void>
-			struct SingleSidedSearch_ : SingleSidedSearch_<Lower, std::integral_constant<bool, Predicate<SafeDouble_<Lower>::value>::value>>{};
-
-			template <std::intmax_t Lower>
-			struct SingleSidedSearch_<Lower, std::false_type> : DoubleSidedSearch_<Lower, SafeDouble_<Lower>::value> {};
-
-			template <std::intmax_t Lower>
-			struct SingleSidedSearch_<Lower, std::true_type> : SingleSidedSearch_<SafeDouble_<Lower>::value>{};
-
-			static constexpr const std::intmax_t value = SingleSidedSearch_<1>::value;
- 		};
-
-		template <template <std::intmax_t N> class Predicate>
-		struct BinarySearch<Predicate, std::enable_if_t<!Predicate<1>::value>> : std::integral_constant<std::intmax_t, 0>{};
-
-		// Find largest std::integer N such that N<=sqrt(R)
-		template <typename R>
-		struct Integer {
-			template <std::intmax_t N> using Predicate_ = std::ratio_less_equal<std::ratio<N>, std::ratio_divide<R, std::ratio<N>>>;
-			static constexpr const std::intmax_t value = BinarySearch<Predicate_>::value;
-		};
-
-		template <typename R>
-		struct IsPerfectSquare {
-			static constexpr const std::intmax_t DenSqrt_ = Integer<std::ratio<R::den>>::value;
-			static constexpr const std::intmax_t NumSqrt_ = Integer<std::ratio<R::num>>::value;
-			static constexpr const bool value =( DenSqrt_ * DenSqrt_ == R::den && NumSqrt_ * NumSqrt_ == R::num);
-			using Sqrt = std::ratio<NumSqrt_, DenSqrt_>;
-		};
-
-		// Represents sqrt(P)-Q.
-		template <typename Tp, typename Tq>
-		struct Remainder {
-			using P = Tp;
-			using Q = Tq;
-		};
-
-		// Represents 1/R = I + Rem where R is a Remainder.
-		template <typename R>
-		struct Reciprocal {
-			using P_ = typename R::P;
-			using Q_ = typename R::Q;
-			using Den_ = std::ratio_subtract<P_, Square<Q_>>;
-			using A_ = std::ratio_divide<Q_, Den_>;
-			using B_ = std::ratio_divide<P_, Square<Den_>>;
-			static constexpr const std::intmax_t I_ = (A_::num + Integer<std::ratio_multiply<B_, Square<std::ratio<A_::den>>>>::value) / A_::den;
-			using I = std::ratio<I_>;
-			using Rem = Remainder<B_, std::ratio_subtract<I, A_>>;
-		};
-
-		// Expands sqrt(R) to continued fraction:
-		// f(x)=C1+1/(C2+1/(C3+1/(...+1/(Cn+x)))) = (U*x+V)/(W*x+1) and sqrt(R)=f(Rem).
-		// The error |f(Rem)-V| = |(U-W*V)x/(W*x+1)| <= |U-W*V|*Rem <= |U-W*V|/I' where
-		// I' is the std::integer part of reciprocal of Rem.
-		template <typename Tr, std::intmax_t N>
-		struct ContinuedFraction {
-			template <typename T>
-			using Abs_ = std::conditional_t<std::ratio_less<T, Zero>::value, std::ratio_subtract<Zero, T>, T>;
-
-			using R = Tr;
-			using Last_ = ContinuedFraction<R, N - 1>;
-			using Reciprocal_ = Reciprocal<typename Last_::Rem>;
-			using Rem = typename Reciprocal_::Rem;
-			using I_ = typename Reciprocal_::I;
-			using Den_ = std::ratio_add<typename Last_::W, I_>;
-			using U = std::ratio_divide<typename Last_::V, Den_>;
-			using V = std::ratio_divide<std::ratio_add<typename Last_::U, std::ratio_multiply<typename Last_::V, I_>>, Den_>;
-			using W = std::ratio_divide<One, Den_>;
-			using Error = Abs_<std::ratio_divide<std::ratio_subtract<U, std::ratio_multiply<V, W>>, typename Reciprocal<Rem>::I>>;
-		};
-
-		template <typename Tr>
-		struct ContinuedFraction<Tr, 1> {
-			using R = Tr;
-			using U = One;
-			using V = std::ratio<Integer<R>::value>;
-			using W = Zero;
-			using Rem = Remainder<R, V>;
-			using Error = std::ratio_divide<One, typename Reciprocal<Rem>::I>;
-		};
-
-		template <typename R, typename Eps, std::intmax_t N = 1, typename enabled = void>
-		struct Sqrt_ : Sqrt_<R, Eps, N + 1> {};
-
-		template <typename R, typename Eps, std::intmax_t N>
-		struct Sqrt_<R, Eps, N, std::enable_if_t<std::ratio_less_equal<typename ContinuedFraction<R, N>::Error, Eps>::value>> {
-			using type = typename ContinuedFraction<R, N>::V;
-		};
-
-		template <typename R, typename Eps, typename enabled = void>
-		struct Sqrt {
-			static_assert(std::ratio_greater_equal<R, Zero>::value, "R can't be negative");
-		};
-
-		template <typename R, typename Eps>
-		struct Sqrt<R, Eps, std::enable_if_t<std::ratio_greater_equal<R, Zero>::value && IsPerfectSquare<R>::value>> {
-			using type = typename IsPerfectSquare<R>::Sqrt;
-		};
-
-		template <typename R, typename Eps>
-		struct Sqrt<R, Eps, std::enable_if_t<(std::ratio_greater_equal<R, Zero>::value && !IsPerfectSquare<R>::value)>> : Sqrt_<R, Eps>{};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @ingroup		TypeTraits
-	 * @brief		Calculate square root of a ratio at compile-time
-	 * @details		Calculates a rational approximation of the square root of the ratio. The error
-	 *				in the calculation is bounded by 1/epsilon (Eps). E.g. for the default value
-	 *				of 10000000000, the maximum error will be a/10000000000, or 1e-8, or said another way,
-	 *				the error will be on the order of 10^-9. Since these calculations are done at 
-	 *				compile time, it is advisable to set epsilon to the highest value that does not
-	 *				cause an integer overflow in the calculation. If you can't compile `ratio_sqrt` 
-	 *				due to overflow errors, reducing the value of epsilon sufficiently will correct
-	 *				the problem.\n\n
-	 *				`ratio_sqrt` is guaranteed to converge for all values of `Ratio` which do not
-	 *				overflow. 
-	 * @note		This function provides a rational approximation, _NOT_ an exact value.
-	 * @tparam		Ratio	ratio to take the square root of. This can represent any rational value,
-	 *						_not_ just integers or values with integer roots.
-	 * @tparam		Eps		Value of epsilon, which represents the inverse of the maximum allowable
-	 *						error. This value should be chosen to be as high as possible before
-	 *						integer overflow errors occur in the compiler.
-	 */
-	template<typename Ratio, std::intmax_t Eps = 10000000000>
-	using ratio_sqrt = typename  units::detail::Sqrt<Ratio, std::ratio<1, Eps>>::type;
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		implementation of `sqrt`
-		 * @details		square roots the conversion ratio, `base_unit` exponents, pi exponents, and removes
-		 *				datum translation ratios.
-		 */
-		template<class Unit, std::intmax_t Eps>
-		struct sqrt_impl
-		{
-			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
-			using Conversion = typename Unit::conversion_ratio;
-			using type = unit <ratio_sqrt<Conversion, Eps>,
-				sqrt_base<traits::base_unit_of<typename Unit::base_unit_type>>,
-				std::ratio_divide<typename Unit::pi_exponent_ratio, std::ratio<2>>,
-				typename Unit::translation_ratio>;
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**	 
-	 * @ingroup		UnitManipulators
-	 * @brief		represents the square root of type `class U`.
-	 * @details		Calculates a rational approximation of the square root of the unit. The error
-	 *				in the calculation is bounded by 1/epsilon (Eps). E.g. for the default value
-	 *				of 10000000000, the maximum error will be a/10000000000, or 1e-8, or said another way,
-	 *				the error will be on the order of 10^-9. Since these calculations are done at
-	 *				compile time, it is advisable to set epsilon to the highest value that does not
-	 *				cause an integer overflow in the calculation. If you can't compile `ratio_sqrt`
-	 *				due to overflow errors, reducing the value of epsilon sufficiently will correct
-	 *				the problem.\n\n
-	 *				`ratio_sqrt` is guaranteed to converge for all values of `Ratio` which do not
-	 *				overflow.
-	 * @tparam		U	`unit` type to take the square root of.
-	 * @tparam		Eps	Value of epsilon, which represents the inverse of the maximum allowable
-	 *					error. This value should be chosen to be as high as possible before
-	 *					integer overflow errors occur in the compiler. 
-	 * @note		USE WITH CAUTION. The is an approximate value. In general, squared<sqrt<meter>> != meter,
-	 *				i.e. the operation is not reversible, and it will result in propogated approximations.
-	 *				Use only when absolutely necessary.
-	 */
-	template<class U, std::intmax_t Eps = 10000000000>
-	using square_root = typename units::detail::sqrt_impl<U, Eps>::type;
-
-	//------------------------------
-	//	COMPOUND UNITS
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-			 * @brief		implementation of compound_unit
-			 * @details		multiplies a variadic list of units together, and is inherited from the resulting
-			 *				type.
-			 */
-		template<class U, class... Us> struct compound_impl;
-		template<class U> struct compound_impl<U> { using type = U; };
-		template<class U1, class U2, class...Us>
-		struct compound_impl<U1, U2, Us...>
-			: compound_impl<unit_multiply<U1, U2>, Us...> {};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @brief		Represents a unit type made up from other units.
-	 * @details		Compound units are formed by multiplying the units of all the types provided in
-	 *				the template argument. Types provided must inherit from `unit`. A compound unit can
-	 *				be formed from any number of other units, and unit manipulators like `inverse` and
-	 *				`squared` are supported. E.g. to specify acceleration, on could create
-	 *				`using acceleration = compound_unit<length::meters, inverse<squared<seconds>>;`
-	 * @tparam		U...	units which, when multiplied together, form the desired compound unit.
-	 * @ingroup		UnitTypes
-	 */
-	template<class U, class... Us>
-	using compound_unit = typename units::detail::compound_impl<U, Us...>::type;
-
-	//------------------------------
-	//	PREFIXES
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/**
-		 * @brief		prefix applicator.
-		 * @details		creates a unit type from a prefix and a unit
-		 */
-		template<class Ratio, class Unit>
-		struct prefix
-		{
-			static_assert(traits::is_ratio<Ratio>::value, "Template parameter `Ratio` must be a `std::ratio`.");
-			static_assert(traits::is_unit<Unit>::value, "Template parameter `Unit` must be a `unit` type.");
-			typedef typename units::unit<Ratio, Unit> type;
-		};
-
-		/// recursive exponential implementation
-		template <int N, class U>
-		struct power_of_ratio
-		{
-			typedef std::ratio_multiply<U, typename power_of_ratio<N - 1, U>::type> type;
-		};
-
-		/// End recursion
-		template <class U>
-		struct power_of_ratio<1, U>
-		{
-			typedef U type;
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @ingroup UnitManipulators
-	 * @{
-	 * @ingroup Decimal Prefixes
-	 * @{
-	 */
-	template<class U> using atto	= typename units::detail::prefix<std::atto,	U>::type;			///< Represents the type of `class U` with the metric 'atto' prefix appended.	@details E.g. atto<meters> represents meters*10^-18		@tparam U unit type to apply the prefix to.
-	template<class U> using femto	= typename units::detail::prefix<std::femto,U>::type;			///< Represents the type of `class U` with the metric 'femto' prefix appended.  @details E.g. femto<meters> represents meters*10^-15	@tparam U unit type to apply the prefix to.
-	template<class U> using pico	= typename units::detail::prefix<std::pico,	U>::type;			///< Represents the type of `class U` with the metric 'pico' prefix appended.	@details E.g. pico<meters> represents meters*10^-12		@tparam U unit type to apply the prefix to.
-	template<class U> using nano	= typename units::detail::prefix<std::nano,	U>::type;			///< Represents the type of `class U` with the metric 'nano' prefix appended.	@details E.g. nano<meters> represents meters*10^-9		@tparam U unit type to apply the prefix to.
-	template<class U> using micro	= typename units::detail::prefix<std::micro,U>::type;			///< Represents the type of `class U` with the metric 'micro' prefix appended.	@details E.g. micro<meters> represents meters*10^-6		@tparam U unit type to apply the prefix to.
-	template<class U> using milli	= typename units::detail::prefix<std::milli,U>::type;			///< Represents the type of `class U` with the metric 'milli' prefix appended.	@details E.g. milli<meters> represents meters*10^-3		@tparam U unit type to apply the prefix to.
-	template<class U> using centi	= typename units::detail::prefix<std::centi,U>::type;			///< Represents the type of `class U` with the metric 'centi' prefix appended.	@details E.g. centi<meters> represents meters*10^-2		@tparam U unit type to apply the prefix to.
-	template<class U> using deci	= typename units::detail::prefix<std::deci,	U>::type;			///< Represents the type of `class U` with the metric 'deci' prefix appended.	@details E.g. deci<meters> represents meters*10^-1		@tparam U unit type to apply the prefix to.
-	template<class U> using deca	= typename units::detail::prefix<std::deca,	U>::type;			///< Represents the type of `class U` with the metric 'deca' prefix appended.	@details E.g. deca<meters> represents meters*10^1		@tparam U unit type to apply the prefix to.
-	template<class U> using hecto	= typename units::detail::prefix<std::hecto,U>::type;			///< Represents the type of `class U` with the metric 'hecto' prefix appended.	@details E.g. hecto<meters> represents meters*10^2		@tparam U unit type to apply the prefix to.
-	template<class U> using kilo	= typename units::detail::prefix<std::kilo,	U>::type;			///< Represents the type of `class U` with the metric 'kilo' prefix appended.	@details E.g. kilo<meters> represents meters*10^3		@tparam U unit type to apply the prefix to.
-	template<class U> using mega	= typename units::detail::prefix<std::mega,	U>::type;			///< Represents the type of `class U` with the metric 'mega' prefix appended.	@details E.g. mega<meters> represents meters*10^6		@tparam U unit type to apply the prefix to.
-	template<class U> using giga	= typename units::detail::prefix<std::giga,	U>::type;			///< Represents the type of `class U` with the metric 'giga' prefix appended.	@details E.g. giga<meters> represents meters*10^9		@tparam U unit type to apply the prefix to.
-	template<class U> using tera	= typename units::detail::prefix<std::tera,	U>::type;			///< Represents the type of `class U` with the metric 'tera' prefix appended.	@details E.g. tera<meters> represents meters*10^12		@tparam U unit type to apply the prefix to.
-	template<class U> using peta	= typename units::detail::prefix<std::peta,	U>::type;			///< Represents the type of `class U` with the metric 'peta' prefix appended.	@details E.g. peta<meters> represents meters*10^15		@tparam U unit type to apply the prefix to.
-	template<class U> using exa		= typename units::detail::prefix<std::exa,	U>::type;			///< Represents the type of `class U` with the metric 'exa' prefix appended.	@details E.g. exa<meters> represents meters*10^18		@tparam U unit type to apply the prefix to.
-	/** @} @} */
-
-	/**
-	 * @ingroup UnitManipulators
-	 * @{
-	 * @ingroup Binary Prefixes
-	 * @{
-	 */
-	template<class U> using kibi	= typename units::detail::prefix<std::ratio<1024>,					U>::type;	///< Represents the type of `class U` with the binary 'kibi' prefix appended.	@details E.g. kibi<bytes> represents bytes*2^10	@tparam U unit type to apply the prefix to.
-	template<class U> using mebi	= typename units::detail::prefix<std::ratio<1048576>,				U>::type;	///< Represents the type of `class U` with the binary 'mibi' prefix appended.	@details E.g. mebi<bytes> represents bytes*2^20	@tparam U unit type to apply the prefix to.
-	template<class U> using gibi	= typename units::detail::prefix<std::ratio<1073741824>,			U>::type;	///< Represents the type of `class U` with the binary 'gibi' prefix appended.	@details E.g. gibi<bytes> represents bytes*2^30	@tparam U unit type to apply the prefix to.
-	template<class U> using tebi	= typename units::detail::prefix<std::ratio<1099511627776>,			U>::type;	///< Represents the type of `class U` with the binary 'tebi' prefix appended.	@details E.g. tebi<bytes> represents bytes*2^40	@tparam U unit type to apply the prefix to.
-	template<class U> using pebi	= typename units::detail::prefix<std::ratio<1125899906842624>,		U>::type;	///< Represents the type of `class U` with the binary 'pebi' prefix appended.	@details E.g. pebi<bytes> represents bytes*2^50	@tparam U unit type to apply the prefix to.
-	template<class U> using exbi	= typename units::detail::prefix<std::ratio<1152921504606846976>,	U>::type;	///< Represents the type of `class U` with the binary 'exbi' prefix appended.	@details E.g. exbi<bytes> represents bytes*2^60	@tparam U unit type to apply the prefix to.
-	/** @} @} */
-
-	//------------------------------
-	//	CONVERSION TRAITS
-	//------------------------------
-
-	namespace traits
-	{
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which checks whether two units can be converted to each other
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_convertible_unit<U1, U2>::value` to test
-		 *				whether `class U1` is convertible to `class U2`. Note: convertible has both the semantic meaning,
-		 *				(i.e. meters can be converted to feet), and the c++ meaning of conversion (type meters can be
-		 *				converted to type feet). Conversion is always symmetric, so if U1 is convertible to U2, then
-		 *				U2 will be convertible to U1.
-		 * @tparam		U1 Unit to convert from.
-		 * @tparam		U2 Unit to convert to.
-		 * @sa			is_convertible_unit_t
-		 */
-		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 >> {};
-	}
-
-	//------------------------------
-	//	CONVERSION FUNCTION
-	//------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		constexpr inline UNIT_LIB_DEFAULT_TYPE pow(UNIT_LIB_DEFAULT_TYPE x, unsigned long long y)
-		{
-			return y == 0 ? 1.0 : x * pow(x, y - 1);
-		}
-
-		constexpr inline UNIT_LIB_DEFAULT_TYPE abs(UNIT_LIB_DEFAULT_TYPE x)
-		{
-			return x < 0 ? -x : x;
-		}
-
-		/// convert dispatch for units which are both the same
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr T convert(const T& value, std::true_type, std::false_type, std::false_type) noexcept
-		{
-			return value;
-		}
-
-		/// convert dispatch for units which are both the same
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr T convert(const T& value, std::true_type, std::false_type, std::true_type) noexcept
-		{
-			return value;
-		}
-
-		/// convert dispatch for units which are both the same
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr T convert(const T& value, std::true_type, std::true_type, std::false_type) noexcept
-		{
-			return value;
-		}
-
-		/// convert dispatch for units which are both the same
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr T convert(const T& value, std::true_type, std::true_type, std::true_type) noexcept
-		{
-			return value;
-		}
-
-		/// convert dispatch for units of different types w/ no translation and no PI
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr T convert(const T& value, std::false_type, std::false_type, std::false_type) noexcept
-		{
-			return ((value * Ratio::num) / Ratio::den);
-		}
-
-		/// convert dispatch for units of different types w/ no translation, but has PI in numerator
-		// constepxr with PI in numerator
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr
-		std::enable_if_t<(PiRatio::num / PiRatio::den >= 1 && PiRatio::num % PiRatio::den == 0), T>
-		convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept
-		{
-			return ((value * pow(constants::detail::PI_VAL, PiRatio::num / PiRatio::den) * Ratio::num) / Ratio::den);
-		}
-
-		/// convert dispatch for units of different types w/ no translation, but has PI in denominator
-		// constexpr with PI in denominator
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr
-		std::enable_if_t<(PiRatio::num / PiRatio::den <= -1 && PiRatio::num % PiRatio::den == 0), T>
- 		convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept
- 		{
- 			return (value * Ratio::num) / (Ratio::den * pow(constants::detail::PI_VAL, -PiRatio::num / PiRatio::den));
- 		}
-
-		/// convert dispatch for units of different types w/ no translation, but has PI in numerator
-		// Not constexpr - uses std::pow
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline // sorry, this can't be constexpr!
-		std::enable_if_t<(PiRatio::num / PiRatio::den < 1 && PiRatio::num / PiRatio::den > -1), T>
-		convert(const T& value, std::false_type, std::true_type, std::false_type) noexcept
-		{
-			return ((value * std::pow(constants::detail::PI_VAL, PiRatio::num / PiRatio::den)  * Ratio::num) / Ratio::den);
-		}
-
-		/// convert dispatch for units of different types with a translation, but no PI
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr T convert(const T& value, std::false_type, std::false_type, std::true_type) noexcept
-		{
-			return ((value * Ratio::num) / Ratio::den) + (static_cast<UNIT_LIB_DEFAULT_TYPE>(Translation::num) / Translation::den);
-		}
-
-		/// convert dispatch for units of different types with a translation AND PI
-		template<class UnitFrom, class UnitTo, class Ratio, class PiRatio, class Translation, typename T>
-		static inline constexpr T convert(const T& value, const std::false_type, const std::true_type, const std::true_type) noexcept
-		{
-			return ((value * std::pow(constants::detail::PI_VAL, PiRatio::num / PiRatio::den) * Ratio::num) / Ratio::den) + (static_cast<UNIT_LIB_DEFAULT_TYPE>(Translation::num) / Translation::den);
-		}
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @ingroup		Conversion
-	 * @brief		converts a <i>value</i> from one type to another.
-	 * @details		Converts a <i>value</i> of a built-in arithmetic type to another unit. This does not change
-	 *				the type of <i>value</i>, only what it contains. E.g. @code double result = convert<length::meters, length::feet>(1.0);	// result == 3.28084 @endcode
-	 * @sa			unit_t	for implicit conversion of unit containers.
-	 * @tparam		UnitFrom unit tag to convert <i>value</i> from. Must be a `unit` type (i.e. is_unit<UnitFrom>::value == true),
-	 *				and must be convertible to `UnitTo` (i.e. is_convertible_unit<UnitFrom, UnitTo>::value == true).
-	 * @tparam		UnitTo unit tag to convert <i>value</i> to. Must be a `unit` type (i.e. is_unit<UnitTo>::value == true),
-	 *				and must be convertible from `UnitFrom` (i.e. is_convertible_unit<UnitFrom, UnitTo>::value == true).
-	 * @tparam		T type of <i>value</i>. It is inferred from <i>value</i>, and is expected to be a built-in arithmetic type.
-	 * @param[in]	value Arithmetic value to convert from `UnitFrom` to `UnitTo`. The value should represent
-	 *				a quantity in units of `UnitFrom`.
-	 * @returns		value, converted from units of `UnitFrom` to `UnitTo`.
-	 */
-	template<class UnitFrom, class UnitTo, typename T = UNIT_LIB_DEFAULT_TYPE>
-	static inline constexpr T convert(const T& value) noexcept
-	{
-		static_assert(traits::is_unit<UnitFrom>::value, "Template parameter `UnitFrom` must be a `unit` type.");
-		static_assert(traits::is_unit<UnitTo>::value, "Template parameter `UnitTo` must be a `unit` type.");
-		static_assert(traits::is_convertible_unit<UnitFrom, UnitTo>::value, "Units are not compatible.");
-
-		using Ratio = std::ratio_divide<typename UnitFrom::conversion_ratio, typename UnitTo::conversion_ratio>;
-		using PiRatio = std::ratio_subtract<typename UnitFrom::pi_exponent_ratio, typename UnitTo::pi_exponent_ratio>;
-		using Translation = std::ratio_divide<std::ratio_subtract<typename UnitFrom::translation_ratio, typename UnitTo::translation_ratio>, typename UnitTo::conversion_ratio>;
-
-		using isSame = typename std::is_same<std::decay_t<UnitFrom>, std::decay_t<UnitTo>>::type;
-		using piRequired = std::integral_constant<bool, !(std::is_same<std::ratio<0>, PiRatio>::value)>;
-		using translationRequired = std::integral_constant<bool, !(std::is_same<std::ratio<0>, Translation>::value)>;
-
-		return units::detail::convert<UnitFrom, UnitTo, Ratio, PiRatio, Translation, T>
-			(value, isSame{}, piRequired{}, translationRequired{});
-	}
-
-	//----------------------------------
-	//	NON-LINEAR SCALE TRAITS
-	//----------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace traits
-	{
-		namespace detail
-		{
-			/**
-			* @brief		implementation of has_operator_parenthesis
-			* @details		checks that operator() returns the same type as `Ret`
-			*/
-			template<class T, class Ret>
-			struct has_operator_parenthesis_impl
-			{
-				template<class U>
-				static constexpr auto test(U*) -> decltype(std::declval<U>()()) { return decltype(std::declval<U>()()){}; }
-				template<typename>
-				static constexpr std::false_type test(...) { return std::false_type{}; }
-
-				using type = typename std::is_same<Ret, decltype(test<T>(0))>::type;
-			};
-		}
-
-		/**
-		 * @brief		checks that `class T` has an `operator()` member which returns `Ret`
-		 * @details		used as part of the linear_scale concept.
-		 */
-		template<class T, class Ret>
-		struct has_operator_parenthesis : traits::detail::has_operator_parenthesis_impl<T, Ret>::type {};
-	}
-
-	namespace traits
-	{
-		namespace detail
-		{
-			/**
-			* @brief		implementation of has_value_member
-			* @details		checks for a member named `m_member` with type `Ret`
-			*/
-			template<class T, class Ret>
-			struct has_value_member_impl
-			{
-				template<class U>
-				static constexpr auto test(U* p) -> decltype(p->m_value) { return p->m_value; }
-				template<typename>
-				static constexpr auto test(...)->std::false_type { return std::false_type{}; }
-
-				using type = typename std::is_same<std::decay_t<Ret>, std::decay_t<decltype(test<T>(0))>>::type;
-			};
-		}
-
-		/**
-		 * @brief		checks for a member named `m_member` with type `Ret`
-		 * @details		used as part of the linear_scale concept checker.
-		 */
-		template<class T, class Ret>
-		struct has_value_member : traits::detail::has_value_member_impl<T, Ret>::type {};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace traits
-	{
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests that `class T` meets the requirements for a non-linear scale
-		 * @details		A non-linear scale must:
-		 *				- be default constructible
-		 *				- have an `operator()` member which returns the non-linear value stored in the scale
-		 *				- have an accessible `m_value` member type which stores the linearized value in the scale.
-		 *
-		 *				Linear/nonlinear scales are used by `units::unit` to store values and scale them
-		 *				if they represent things like dB.
-		 */
-		template<class T, class Ret>
-		struct is_nonlinear_scale : std::integral_constant<bool,
-			std::is_default_constructible<T>::value &&
-			has_operator_parenthesis<T, Ret>::value &&
-			has_value_member<T, Ret>::value &&
-			std::is_trivial<T>::value>
-		{};
-	}
-
-	//------------------------------
-	//	UNIT_T TYPE TRAITS
-	//------------------------------
-
-	namespace traits
-	{
-#ifdef FOR_DOXYGEN_PURPOSOES_ONLY
-		/**
-		* @ingroup		TypeTraits
-		* @brief		Trait for accessing the publically defined types of `units::unit_t`
-		* @details		The units library determines certain properties of the unit_t types passed to them
-		*				and what they represent by using the members of the corresponding unit_t_traits instantiation.
-		*/
-		template<typename T>
-		struct unit_t_traits
-		{
-			typedef typename T::non_linear_scale_type non_linear_scale_type;	///< Type of the unit_t non_linear_scale (e.g. linear_scale, decibel_scale). This property is used to enable the proper linear or logarithmic arithmetic functions.
-			typedef typename T::underlying_type underlying_type;				///< Underlying storage type of the `unit_t`, e.g. `double`.
-			typedef typename T::value_type value_type;							///< Synonym for underlying type. May be removed in future versions. Prefer underlying_type.
-			typedef typename T::unit_type unit_type;							///< Type of unit the `unit_t` represents, e.g. `meters`
-		};
-#endif
-
-		/** @cond */	// DOXYGEN IGNORE
-		/**
-		 * @brief		unit_t_traits specialization for things which are not unit_t
-		 * @details
-		 */
-		template<typename T, typename = void>
-		struct unit_t_traits
-		{
-			typedef void non_linear_scale_type;
-			typedef void underlying_type;
-			typedef void value_type;
-			typedef void unit_type;
-		};
-	
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait for accessing the publically defined types of `units::unit_t`
-		 * @details
-		 */
-		template<typename T>
-		struct unit_t_traits <T, typename void_t<
-			typename T::non_linear_scale_type,
-			typename T::underlying_type,
-			typename T::value_type,
-			typename T::unit_type>::type>
-		{
-			typedef typename T::non_linear_scale_type non_linear_scale_type;
-			typedef typename T::underlying_type underlying_type;
-			typedef typename T::value_type value_type;
-			typedef typename T::unit_type unit_type;
-		};
-		/** @endcond */	// END DOXYGEN IGNORE
-	}
-
-	namespace traits
-	{
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests whether two container types derived from `unit_t` are convertible to each other
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_convertible_unit_t<U1, U2>::value` to test
-		 *				whether `class U1` is convertible to `class U2`. Note: convertible has both the semantic meaning,
-		 *				(i.e. meters can be converted to feet), and the c++ meaning of conversion (type meters can be
-		 *				converted to type feet). Conversion is always symmetric, so if U1 is convertible to U2, then
-		 *				U2 will be convertible to U1.
-		 * @tparam		U1 Unit to convert from.
-		 * @tparam		U2 Unit to convert to.
-		 * @sa			is_convertible_unit
-		 */
-		template<class U1, class U2>
-		struct is_convertible_unit_t : std::integral_constant<bool,
-			is_convertible_unit<typename units::traits::unit_t_traits<U1>::unit_type, typename units::traits::unit_t_traits<U2>::unit_type>::value>
-		{};
-	}
-
-	//---------------------------------- 
-	//	UNIT TYPE
-	//----------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	// forward declaration
-	template<typename T> struct linear_scale;
-	template<typename T> struct decibel_scale;
-
-	namespace detail
-	{
-		/**
-		* @brief		helper type to identify units.
-		* @details		A non-templated base class for `unit` which enables RTTI testing.
-		*/
-		struct _unit_t {};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace traits
-	{
-		// forward declaration
-		#if !defined(_MSC_VER) || _MSC_VER > 1800 // bug in VS2013 prevents this from working
-		template<typename... T> struct is_dimensionless_unit;
-		#else
-		template<typename T1, typename T2 = T1, typename T3 = T1> struct is_dimensionless_unit;
-		#endif
-
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Traits which tests if a class is a `unit`
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_unit<T>::value` to test
-		 *				whether `class T` implements a `unit`.
-		 */
-		template<class T>
-		struct is_unit_t : std::is_base_of<units::detail::_unit_t, T>::type {};
-	}
-
-	/**
-	 * @ingroup		UnitContainers
-	 * @brief		Container for values which represent quantities of a given unit.
-	 * @details		Stores a value which represents a quantity in the given units. Unit containers
-	 *				(except scalar values) are *not* convertible to built-in c++ types, in order to
-	 *				provide type safety in dimensional analysis. Unit containers *are* implicitly
-	 *				convertible to other compatible unit container types. Unit containers support
-	 *				various types of arithmetic operations, depending on their scale type.
-	 *
-	 *				The value of a `unit_t` can only be changed on construction, or by assignment
-	 *				from another `unit_t` type. If necessary, the underlying value can be accessed
-	 *				using `operator()`: @code
-	 *				meter_t m(5.0);
-	 *				double val = m(); // val == 5.0	@endcode.
-	 * @tparam		Units unit tag for which type of units the `unit_t` represents (e.g. meters)
-	 * @tparam		T underlying type of the storage. Defaults to double.
-	 * @tparam		NonLinearScale optional scale class for the units. Defaults to linear (i.e. does
-	 *				not scale the unit value). Examples of non-linear scales could be logarithmic,
-	 *				decibel, or richter scales. Non-linear scales must adhere to the non-linear-scale
-	 *				concept, i.e. `is_nonlinear_scale<...>::value` must be `true`.
-	 * @sa
-	 *				- \ref lengthContainers "length unit containers"
-	 *				- \ref massContainers "mass unit containers"
-	 *				- \ref timeContainers "time unit containers"
-	 *				- \ref angleContainers "angle unit containers"
-	 *				- \ref currentContainers "current unit containers"
-	 *				- \ref temperatureContainers "temperature unit containers"
-	 *				- \ref substanceContainers "substance unit containers"
-	 *				- \ref luminousIntensityContainers "luminous intensity unit containers"
-	 *				- \ref solidAngleContainers "solid angle unit containers"
-	 *				- \ref frequencyContainers "frequency unit containers"
-	 *				- \ref velocityContainers "velocity unit containers"
-	 *				- \ref angularVelocityContainers "angular velocity unit containers"
-	 *				- \ref accelerationContainers "acceleration unit containers"
-	 *				- \ref forceContainers "force unit containers"
-	 *				- \ref pressureContainers "pressure unit containers"
-	 *				- \ref chargeContainers "charge unit containers"
-	 *				- \ref energyContainers "energy unit containers"
-	 *				- \ref powerContainers "power unit containers"
-	 *				- \ref voltageContainers "voltage unit containers"
-	 *				- \ref capacitanceContainers "capacitance unit containers"
-	 *				- \ref impedanceContainers "impedance unit containers"
-	 *				- \ref magneticFluxContainers "magnetic flux unit containers"
-	 *				- \ref magneticFieldStrengthContainers "magnetic field strength unit containers"
-	 *				- \ref inductanceContainers "inductance unit containers"
-	 *				- \ref luminousFluxContainers "luminous flux unit containers"
-	 *				- \ref illuminanceContainers "illuminance unit containers"
-	 *				- \ref radiationContainers "radiation unit containers"
-	 *				- \ref torqueContainers "torque unit containers"
-	 *				- \ref areaContainers "area unit containers"
-	 *				- \ref volumeContainers "volume unit containers"
-	 *				- \ref densityContainers "density unit containers"
-	 *				- \ref concentrationContainers "concentration unit containers"
-	 *				- \ref constantContainers "constant unit containers"
-	 */
-	template<class Units, typename T = UNIT_LIB_DEFAULT_TYPE, template<typename> class NonLinearScale = linear_scale>
-	class unit_t : public NonLinearScale<T>, units::detail::_unit_t
-	{
-		static_assert(traits::is_unit<Units>::value, "Template parameter `Units` must be a unit tag. Check that you aren't using a unit type (_t).");
-		static_assert(traits::is_nonlinear_scale<NonLinearScale<T>, T>::value, "Template parameter `NonLinearScale` does not conform to the `is_nonlinear_scale` concept.");
-
-	protected:
-
-		using nls = NonLinearScale<T>;
-		using nls::m_value;
-
-	public:
-
-		typedef NonLinearScale<T> non_linear_scale_type;											///< Type of the non-linear scale of the unit_t (e.g. linear_scale)
-		typedef T underlying_type;																	///< Type of the underlying storage of the unit_t (e.g. double)
-		typedef T value_type;																		///< Synonym for underlying type. May be removed in future versions. Prefer underlying_type.
-		typedef Units unit_type;																	///< Type of `unit` the `unit_t` represents (e.g. meters)
-
-		/**
-		 * @ingroup		Constructors
-		 * @brief		default constructor.
-		 */
-		constexpr unit_t() = default;
-
-		/**
-		 * @brief		constructor
-		 * @details		constructs a new unit_t using the non-linear scale's constructor.
-		 * @param[in]	value	unit value magnitude.
-		 * @param[in]	args	additional constructor arguments are forwarded to the non-linear scale constructor. Which
-		 *						args are required depends on which scale is used. For the default (linear) scale,
-		 *						no additional args are necessary.
-		 */
-		template<class... Args>
-		inline explicit constexpr unit_t(const T value, const Args&... args) noexcept : nls(value, args...) 
-		{
-
-		}
-
-		/**
-		 * @brief		constructor
-		 * @details		enable implicit conversions from T types ONLY for linear scalar units
-		 * @param[in]	value value of the unit_t
-		 */
-		template<class Ty, class = typename std::enable_if<traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value>::type>
-		inline constexpr unit_t(const Ty value) noexcept : nls(value) 
-		{
-
-		}
-
-		/**
-		 * @brief		chrono constructor
-		 * @details		enable implicit conversions from std::chrono::duration types ONLY for time units
-		 * @param[in]	value value of the unit_t
-		 */
-		template<class Rep, class Period, class = std::enable_if_t<std::is_arithmetic<Rep>::value && traits::is_ratio<Period>::value>>
-		inline constexpr unit_t(const std::chrono::duration<Rep, Period>& value) noexcept : 
-		nls(units::convert<unit<std::ratio<1,1000000000>, category::time_unit>, Units>(static_cast<T>(std::chrono::duration_cast<std::chrono::nanoseconds>(value).count()))) 
-		{
-
-		}
-
-		/**
-		 * @brief		copy constructor (converting)
-		 * @details		performs implicit unit conversions if required.
-		 * @param[in]	rhs unit to copy.
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
-		inline constexpr unit_t(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) noexcept :
-		nls(units::convert<UnitsRhs, Units, T>(rhs.m_value), std::true_type() /*store linear value*/)
-		{
-
-		}
-
-		/**
-		 * @brief		assignment
-		 * @details		performs implicit unit conversions if required
-		 * @param[in]	rhs unit to copy.
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
-		inline unit_t& operator=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) noexcept
-		{		
-			nls::m_value = units::convert<UnitsRhs, Units, T>(rhs.m_value);
-			return *this;
-		}
-
-		/**
-		* @brief		assignment
-		* @details		performs implicit conversions from built-in types ONLY for scalar units
-		* @param[in]	rhs value to copy.
-		*/
-		template<class Ty, class = std::enable_if_t<traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value>>
-		inline unit_t& operator=(const Ty& rhs) noexcept
-		{
-			nls::m_value = rhs;
-			return *this;
-		}
-
-		/**
-		 * @brief		less-than
-		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
-		 * @param[in]	rhs right-hand side unit for the comparison
-		 * @returns		true IFF the value of `this` is less than the value of `rhs`
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
-		inline constexpr bool operator<(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
-		{
-			return (nls::m_value < units::convert<UnitsRhs, Units>(rhs.m_value));
-		}
-
-		/**
-		 * @brief		less-than or equal
-		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
-		 * @param[in]	rhs right-hand side unit for the comparison
-		 * @returns		true IFF the value of `this` is less than or equal to the value of `rhs`
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
-		inline constexpr bool operator<=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
-		{
-			return (nls::m_value <= units::convert<UnitsRhs, Units>(rhs.m_value));
-		}
-
-		/**
-		 * @brief		greater-than
-		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
-		 * @param[in]	rhs right-hand side unit for the comparison
-		 * @returns		true IFF the value of `this` is greater than the value of `rhs`
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
-		inline constexpr bool operator>(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
-		{
-			return (nls::m_value > units::convert<UnitsRhs, Units>(rhs.m_value));
-		}
-
-		/**
-		 * @brief		greater-than or equal
-		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
-		 * @param[in]	rhs right-hand side unit for the comparison
-		 * @returns		true IFF the value of `this` is greater than or equal to the value of `rhs`
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
-		inline constexpr bool operator>=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
-		{
-			return (nls::m_value >= units::convert<UnitsRhs, Units>(rhs.m_value));
-		}
-
-		/**
-		 * @brief		equality
-		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
-		 * @param[in]	rhs right-hand side unit for the comparison
-		 * @returns		true IFF the value of `this` exactly equal to the value of rhs.
-		 * @note		This may not be suitable for all applications when the underlying_type of unit_t is a double.
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs, std::enable_if_t<std::is_floating_point<T>::value || std::is_floating_point<Ty>::value, int> = 0>
-		inline constexpr bool operator==(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
-		{
-			return detail::abs(nls::m_value - units::convert<UnitsRhs, Units>(rhs.m_value)) < std::numeric_limits<T>::epsilon() * 
-				detail::abs(nls::m_value + units::convert<UnitsRhs, Units>(rhs.m_value)) ||
-				detail::abs(nls::m_value - units::convert<UnitsRhs, Units>(rhs.m_value)) < std::numeric_limits<T>::min();
-		}
-
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs, std::enable_if_t<std::is_integral<T>::value && std::is_integral<Ty>::value, int> = 0>
-		inline constexpr bool operator==(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
-		{
-			return nls::m_value == units::convert<UnitsRhs, Units>(rhs.m_value);
-		}
-
-		/**
-		 * @brief		inequality
-		 * @details		compares the linearized value of two units. Performs unit conversions if necessary.
-		 * @param[in]	rhs right-hand side unit for the comparison
-		 * @returns		true IFF the value of `this` is not equal to the value of rhs.
-		 * @note		This may not be suitable for all applications when the underlying_type of unit_t is a double.
-		 */
-		template<class UnitsRhs, typename Ty, template<typename> class NlsRhs>
-		inline constexpr bool operator!=(const unit_t<UnitsRhs, Ty, NlsRhs>& rhs) const noexcept
-		{
-			return !(*this == rhs);
-		}
-
-		/**
-		 * @brief		unit value
-		 * @returns		value of the unit in it's underlying, non-safe type.
-		 */
-		inline constexpr underlying_type value() const noexcept
-		{
-			return static_cast<underlying_type>(*this);
-		}
-
-		/**
-		 * @brief		unit value
-		 * @returns		value of the unit converted to an arithmetic, non-safe type.
-		 */
-		template<typename Ty, class = std::enable_if_t<std::is_arithmetic<Ty>::value>>
-		inline constexpr Ty to() const noexcept
-		{
-			return static_cast<Ty>(*this);
-		}
-
-		/**
-		 * @brief		linearized unit value
-		 * @returns		linearized value of unit which has a non-linear scale. For `unit_t` types with
-		 *				linear scales, this is equivalent to `value`.
-		 */
-		template<typename Ty, class = std::enable_if_t<std::is_arithmetic<Ty>::value>>
-		inline constexpr Ty toLinearized() const noexcept
-		{
-			return static_cast<Ty>(m_value);
-		}
-
-		/**
-		 * @brief		conversion
-		 * @details		Converts to a different unit container. Units can be converted to other containers
-		 *				implicitly, but this can be used in cases where explicit notation of a conversion
-		 *				is beneficial, or where an r-value container is needed.
-		 * @tparam		U unit (not unit_t) to convert to
-		 * @returns		a unit container with the specified units containing the equivalent value to
-		 *				*this.
-		 */
-		template<class U>
-		inline constexpr unit_t<U> convert() const noexcept
-		{
-			static_assert(traits::is_unit<U>::value, "Template parameter `U` must be a unit type.");
-			return unit_t<U>(*this);
-		}
-
-		/**
-		 * @brief		implicit type conversion.
-		 * @details		only enabled for scalar unit types.
-		 */
-		template<class Ty, std::enable_if_t<traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value, int> = 0>
-		inline constexpr operator Ty() const noexcept 
-		{ 
-			// this conversion also resolves any PI exponents, by converting from a non-zero PI ratio to a zero-pi ratio.
-			return static_cast<Ty>(units::convert<Units, unit<std::ratio<1>, units::category::scalar_unit>>((*this)()));
-		}
-
-		/**
-		 * @brief		explicit type conversion.
-		 * @details		only enabled for non-dimensionless unit types.
-		 */
-		template<class Ty, std::enable_if_t<!traits::is_dimensionless_unit<Units>::value && std::is_arithmetic<Ty>::value, int> = 0>
-		inline constexpr explicit operator Ty() const noexcept
-		{
-			return static_cast<Ty>((*this)());
-		}
-
-		/**
-		 * @brief		chrono implicit type conversion.
-		 * @details		only enabled for time unit types.
-		 */
-		template<typename U = Units, std::enable_if_t<units::traits::is_convertible_unit<U, unit<std::ratio<1>, category::time_unit>>::value, int> = 0>
-		inline constexpr operator std::chrono::nanoseconds() const noexcept
-		{
-			return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double, std::nano>(units::convert<Units, unit<std::ratio<1,1000000000>, category::time_unit>>((*this)())));
-		}
-
-		/**
-		 * @brief		returns the unit name
-		 */
-		inline constexpr const char* name() const noexcept
-		{
-			return units::name(*this);
-		}
-
-		/**
-		 * @brief		returns the unit abbreviation
-		 */
-		inline constexpr const char* abbreviation() const noexcept
-		{
-			return units::abbreviation(*this);
-		}
-
-	public:
-
-		template<class U, typename Ty, template<typename> class Nlt>
-		friend class unit_t;
-	};
-
-	//------------------------------
-	//	UNIT_T NON-MEMBER FUNCTIONS
-	//------------------------------
-
-	/**
-	 * @ingroup		UnitContainers
-	 * @brief		Constructs a unit container from an arithmetic type.
-	 * @details		make_unit can be used to construct a unit container from an arithmetic type, as an alternative to
-	 *				using the explicit constructor. Unlike the explicit constructor it forces the user to explicitly
-	 *				specify the units.
-	 * @tparam		UnitType Type to construct.
-	 * @tparam		Ty		Arithmetic type.
-	 * @param[in]	value	Arithmetic value that represents a quantity in units of `UnitType`.
-	 */
-	template<class UnitType, typename T, class = std::enable_if_t<std::is_arithmetic<T>::value>>
-	inline constexpr UnitType make_unit(const T value) noexcept
-	{
-		static_assert(traits::is_unit_t<UnitType>::value, "Template parameter `UnitType` must be a unit type (_t).");
-		
-		return UnitType(value);
-	}
-
-#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline std::ostream& operator<<(std::ostream& os, const unit_t<Units, T, NonLinearScale>& obj) noexcept
-	{
-		using BaseUnits = unit<std::ratio<1>, typename traits::unit_traits<Units>::base_unit_type>;
-		os << convert<Units, BaseUnits>(obj());
-
-		if (traits::unit_traits<Units>::base_unit_type::meter_ratio::num != 0) { os << " m"; }
-		if (traits::unit_traits<Units>::base_unit_type::meter_ratio::num != 0 && 
-			traits::unit_traits<Units>::base_unit_type::meter_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::meter_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::meter_ratio::den != 1) { os << "/"   << traits::unit_traits<Units>::base_unit_type::meter_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num != 0) { os << " kg"; }
-		if (traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num != 0 &&
-			traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::kilogram_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::kilogram_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::kilogram_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::second_ratio::num != 0) { os << " s"; }
-		if (traits::unit_traits<Units>::base_unit_type::second_ratio::num != 0 &&
-			traits::unit_traits<Units>::base_unit_type::second_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::second_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::second_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::second_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::ampere_ratio::num != 0) { os << " A"; }
-		if (traits::unit_traits<Units>::base_unit_type::ampere_ratio::num != 0 &&
-			traits::unit_traits<Units>::base_unit_type::ampere_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::ampere_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::ampere_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::ampere_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num != 0) { os << " K"; }
-		if (traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num != 0 &&
-			traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::kelvin_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::kelvin_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::mole_ratio::num != 0) { os << " mol"; }
-		if (traits::unit_traits<Units>::base_unit_type::mole_ratio::num != 0 && 
-			traits::unit_traits<Units>::base_unit_type::mole_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::mole_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::mole_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::mole_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::candela_ratio::num != 0) { os << " cd"; }
-		if (traits::unit_traits<Units>::base_unit_type::candela_ratio::num != 0 &&
-			traits::unit_traits<Units>::base_unit_type::candela_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::candela_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::candela_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::candela_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::radian_ratio::num != 0) { os << " rad"; }
-		if (traits::unit_traits<Units>::base_unit_type::radian_ratio::num != 0 &&
-			traits::unit_traits<Units>::base_unit_type::radian_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::radian_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::radian_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::radian_ratio::den; }
-
-		if (traits::unit_traits<Units>::base_unit_type::byte_ratio::num != 0) { os << " b"; }
-		if (traits::unit_traits<Units>::base_unit_type::byte_ratio::num != 0 &&
-			traits::unit_traits<Units>::base_unit_type::byte_ratio::num != 1) { os << "^" << traits::unit_traits<Units>::base_unit_type::byte_ratio::num; }
-		if (traits::unit_traits<Units>::base_unit_type::byte_ratio::den != 1) { os << "/" << traits::unit_traits<Units>::base_unit_type::byte_ratio::den; }
-
-		return os;
-	}
-#endif
-
-	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
-	inline unit_t<Units, T, NonLinearScale>& operator+=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
-	{
-		static_assert(traits::is_convertible_unit_t<unit_t<Units, T, NonLinearScale>, RhsType>::value ||
-			(traits::is_dimensionless_unit<decltype(lhs)>::value && std::is_arithmetic<RhsType>::value), 
-			"parameters are not compatible units.");
-
-		lhs = lhs + rhs;
-		return lhs;
-	}
-
-	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
-	inline unit_t<Units, T, NonLinearScale>& operator-=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
-	{
-		static_assert(traits::is_convertible_unit_t<unit_t<Units, T, NonLinearScale>, RhsType>::value ||
-			(traits::is_dimensionless_unit<decltype(lhs)>::value && std::is_arithmetic<RhsType>::value),
-			"parameters are not compatible units.");
-
-		lhs = lhs - rhs;
-		return lhs;
-	}
-
-	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
-	inline unit_t<Units, T, NonLinearScale>& operator*=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
-	{
-		static_assert((traits::is_dimensionless_unit<RhsType>::value || std::is_arithmetic<RhsType>::value),
-			"right-hand side parameter must be dimensionless.");
-
-		lhs = lhs * rhs;
-		return lhs;
-	}
-
-	template<class Units, typename T, template<typename> class NonLinearScale, typename RhsType>
-	inline unit_t<Units, T, NonLinearScale>& operator/=(unit_t<Units, T, NonLinearScale>& lhs, const RhsType& rhs) noexcept
-	{
-		static_assert((traits::is_dimensionless_unit<RhsType>::value || std::is_arithmetic<RhsType>::value),
-			"right-hand side parameter must be dimensionless.");
-
-		lhs = lhs / rhs;
-		return lhs;
-	}
-
-	//------------------------------
-	//	UNIT_T UNARY OPERATORS
-	//------------------------------
-
-	// unary addition: +T
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale> operator+(const unit_t<Units, T, NonLinearScale>& u) noexcept
-	{
-		return u;
-	}
-
-	// prefix increment: ++T
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale>& operator++(unit_t<Units, T, NonLinearScale>& u) noexcept
-	{
-		u = unit_t<Units, T, NonLinearScale>(u() + 1);
-		return u;
-	}
-
-	// postfix increment: T++
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale> operator++(unit_t<Units, T, NonLinearScale>& u, int) noexcept
-	{
-		auto ret = u;
-		u = unit_t<Units, T, NonLinearScale>(u() + 1);
-		return ret;
-	}
-
-	// unary addition: -T
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale> operator-(const unit_t<Units, T, NonLinearScale>& u) noexcept
-	{
-		return unit_t<Units, T, NonLinearScale>(-u());
-	}
-
-	// prefix increment: --T
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale>& operator--(unit_t<Units, T, NonLinearScale>& u) noexcept
-	{
-		u = unit_t<Units, T, NonLinearScale>(u() - 1);
-		return u;
-	}
-
-	// postfix increment: T--
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	inline unit_t<Units, T, NonLinearScale> operator--(unit_t<Units, T, NonLinearScale>& u, int) noexcept
-	{
-		auto ret = u;
-		u = unit_t<Units, T, NonLinearScale>(u() - 1);
-		return ret;
-	}
-
-	//------------------------------
-	//	UNIT_CAST
-	//------------------------------	
-	
-	/** 
-	 * @ingroup		Conversion
-	 * @brief		Casts a unit container to an arithmetic type.
-	 * @details		unit_cast can be used to remove the strong typing from a unit class, and convert it
-	 *				to a built-in arithmetic type. This may be useful for compatibility with libraries
-	 *				and legacy code that don't support `unit_t` types. E.g 
-	 * @code		meter_t unitVal(5);
-	 *  double value = units::unit_cast<double>(unitVal);	// value = 5.0 
-	 * @endcode
-	 * @tparam		T		Type to cast the unit type to. Must be a built-in arithmetic type.
-	 * @param		value	Unit value to cast.
-	 * @sa			unit_t::to
-	 */
-	template<typename T, typename Units, class = std::enable_if_t<std::is_arithmetic<T>::value && traits::is_unit_t<Units>::value>>
-	inline constexpr T unit_cast(const Units& value) noexcept
-	{
-		return static_cast<T>(value);
-	}
-
-	//------------------------------
-	//	NON-LINEAR SCALE TRAITS
-	//------------------------------
-
-	// forward declaration
-	template<typename T> struct decibel_scale;
-
-	namespace traits
-	{
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests whether a type is inherited from a linear scale.
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_linear_scale<U1 [, U2, ...]>::value` to test
-		 *				one or more types to see if they represent unit_t's whose scale is linear.
-		 * @tparam		T	one or more types to test.
-		 */
-#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 > {};
-#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> {};
-#endif
-
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests whether a type is inherited from a decibel scale.
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `has_decibel_scale<U1 [, U2, ...]>::value` to test
-		 *				one or more types to see if they represent unit_t's whose scale is in decibels.
-		 * @tparam		T	one or more types to test.
-		 */
-#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> {};
-#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> {};
-#endif
-
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests whether two types has the same non-linear scale.
-		 * @details		Inherits from `std::true_type` or `std::false_type`. Use `is_same_scale<U1 , U2>::value` to test
-		 *				whether two types have the same non-linear scale.
-		 * @tparam		T1	left hand type.
-		 * @tparam		T2	right hand type
-		 */
-		template<typename T1, typename T2>
-		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>
-		{};
-	}
-
-	//----------------------------------
-	//	NON-LINEAR SCALES
-	//----------------------------------
-
-	// Non-linear transforms are used to pre and post scale units which are defined in terms of non-
-	// linear functions of their current value. A good example of a non-linear scale would be a 
-	// logarithmic or decibel scale
-
-	//------------------------------
-	//	LINEAR SCALE
-	//------------------------------
-
-	/**
-	 * @brief		unit_t scale which is linear
-	 * @details		Represents units on a linear scale. This is the appropriate unit_t scale for almost
-	 *				all units almost all of the time.
-	 * @tparam		T	underlying storage type
-	 * @sa			unit_t
-	 */
-	template<typename T>
-	struct linear_scale
-	{
-		inline constexpr linear_scale() = default;													///< default constructor.		
-		inline constexpr linear_scale(const linear_scale&) = default;
-		inline ~linear_scale() = default;
-		inline linear_scale& operator=(const linear_scale&) = default;
-#if defined(_MSC_VER) && (_MSC_VER > 1800)
-		inline constexpr linear_scale(linear_scale&&) = default;
-		inline linear_scale& operator=(linear_scale&&) = default;
-#endif
-		template<class... Args>
-		inline constexpr linear_scale(const T& value, Args&&...) noexcept : m_value(value) {}	///< constructor.
-		inline constexpr T operator()() const noexcept { return m_value; }							///< returns value.
-
-		T m_value;																					///< linearized value.	
-	};
-
-	//----------------------------------
-	//	SCALAR (LINEAR) UNITS
-	//----------------------------------
-
-	// Scalar units are the *ONLY* units implicitly convertible to/from built-in types.
-	namespace dimensionless
-	{
-		typedef unit<std::ratio<1>, units::category::scalar_unit> scalar;
-		typedef unit<std::ratio<1>, units::category::dimensionless_unit> dimensionless;
-
-		typedef unit_t<scalar> scalar_t;
-		typedef scalar_t dimensionless_t;
-	}
-
-// ignore the redeclaration of the default template parameters
-#if defined(_MSC_VER) 
-#	pragma warning(push)
-#	pragma warning(disable : 4348)
-#endif
-	UNIT_ADD_CATEGORY_TRAIT(scalar)
-	UNIT_ADD_CATEGORY_TRAIT(dimensionless)
-#if defined(_MSC_VER) 
-#	pragma warning(pop)
-#endif
-
-	//------------------------------
-	//	LINEAR ARITHMETIC
-	//------------------------------
-
-	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<!traits::is_same_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-	constexpr inline int operator+(const UnitTypeLhs& /* lhs */, const UnitTypeRhs& /* rhs */) noexcept
-	{
-		static_assert(traits::is_same_scale<UnitTypeLhs, UnitTypeRhs>::value, "Cannot add units with different linear/non-linear scales.");
-		return 0;
-	}
-
-	/// Addition operator for unit_t types with a linear_scale.
-	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-	inline constexpr UnitTypeLhs operator+(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		return UnitTypeLhs(lhs() + convert<UnitsRhs, UnitsLhs>(rhs()));
-	}
-
-	/// Addition operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
-	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
-	inline constexpr dimensionless::scalar_t operator+(const dimensionless::scalar_t& lhs, T rhs) noexcept
-	{
-		return dimensionless::scalar_t(lhs() + rhs);
-	}
-
-	/// Addition operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
-	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
-	inline constexpr dimensionless::scalar_t operator+(T lhs, const dimensionless::scalar_t& rhs) noexcept
-	{
-		return dimensionless::scalar_t(lhs + rhs());
-	}
-
-	/// Subtraction operator for unit_t types with a linear_scale.
-	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-	inline constexpr UnitTypeLhs operator-(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		return UnitTypeLhs(lhs() - convert<UnitsRhs, UnitsLhs>(rhs()));
-	}
-
-	/// Subtraction operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
-	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
-	inline constexpr dimensionless::scalar_t operator-(const dimensionless::scalar_t& lhs, T rhs) noexcept
-	{
-		return dimensionless::scalar_t(lhs() - rhs);
-	}
-
-	/// Subtraction operator for scalar unit_t types with a linear_scale. Scalar types can be implicitly converted to built-in types.
-	template<typename T, std::enable_if_t<std::is_arithmetic<T>::value, int> = 0>
-	inline constexpr dimensionless::scalar_t operator-(T lhs, const dimensionless::scalar_t& rhs) noexcept
-	{
-		return dimensionless::scalar_t(lhs - rhs());
-	}
-
-	/// Multiplication type for convertible unit_t types with a linear scale. @returns the multiplied value, with the same type as left-hand side unit.
-	template<class UnitTypeLhs, class UnitTypeRhs,
-		std::enable_if_t<traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-		inline constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<squared<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>>>
-	{
-		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		return  unit_t<compound_unit<squared<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>>>
-			(lhs() * convert<UnitsRhs, UnitsLhs>(rhs()));
-	}
-	
-	/// Multiplication type for non-convertible unit_t types with a linear scale. @returns the multiplied value, whose type is a compound unit of the left and right hand side values.
-	template<class UnitTypeLhs, class UnitTypeRhs,
-		std::enable_if_t<!traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-		inline constexpr auto operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type, typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
-	{
-		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		return unit_t<compound_unit<UnitsLhs, UnitsRhs>>
-			(lhs() * rhs());
-	}
-
-	/// Multiplication by a dimensionless unit for unit_t types with a linear scale.
-	template<class UnitTypeLhs, typename UnitTypeRhs,
-		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-		inline constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		// the cast makes sure factors of PI are handled as expected
-		return UnitTypeLhs(lhs() * static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
-	}
-
-	/// Multiplication by a dimensionless unit for unit_t types with a linear scale.
-	template<class UnitTypeLhs, typename UnitTypeRhs,
-		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-		inline constexpr UnitTypeRhs operator*(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		// the cast makes sure factors of PI are handled as expected
-		return UnitTypeRhs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) * rhs());
-	}
-
-	/// Multiplication by a scalar for unit_t types with a linear scale.
-	template<class UnitTypeLhs, typename T,
-		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeLhs>::value, int> = 0>
-		inline constexpr UnitTypeLhs operator*(const UnitTypeLhs& lhs, T rhs) noexcept
-	{
-		return UnitTypeLhs(lhs() * rhs);
-	}
-
-	/// Multiplication by a scalar for unit_t types with a linear scale.
-	template<class UnitTypeRhs, typename T,
-		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeRhs>::value, int> = 0>
-		inline constexpr UnitTypeRhs operator*(T lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		return UnitTypeRhs(lhs * rhs());
-	}
-
-	/// Division for convertible unit_t types with a linear scale. @returns the lhs divided by rhs value, whose type is a scalar
-	template<class UnitTypeLhs, class UnitTypeRhs,
-		std::enable_if_t<traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-		inline constexpr dimensionless::scalar_t operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		return dimensionless::scalar_t(lhs() / convert<UnitsRhs, UnitsLhs>(rhs()));
-	}
-
-	/// Division for non-convertible unit_t types with a linear scale. @returns the lhs divided by the rhs, with a compound unit type of lhs/rhs 
-	template<class UnitTypeLhs, class UnitTypeRhs,
-		std::enable_if_t<!traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value && traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-		inline constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept ->  unit_t<compound_unit<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type, inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>>
-	{
-		using UnitsLhs = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		return unit_t<compound_unit<UnitsLhs, inverse<UnitsRhs>>>
-			(lhs() / rhs());
-	}
-
-	/// Division by a dimensionless unit for unit_t types with a linear scale
-	template<class UnitTypeLhs, class UnitTypeRhs,
-		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value && traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-		inline constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		return UnitTypeLhs(lhs() / static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
-	}
-
-	/// Division of a dimensionless unit  by a unit_t type with a linear scale
-	template<class UnitTypeLhs, class UnitTypeRhs,
-		std::enable_if_t<traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value && traits::is_dimensionless_unit<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-		inline constexpr auto operator/(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
-	{
-		return unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
-			(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) / rhs());
-	}
-
-	/// Division by a scalar for unit_t types with a linear scale
-	template<class UnitTypeLhs, typename T,
-		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeLhs>::value, int> = 0>
-		inline constexpr UnitTypeLhs operator/(const UnitTypeLhs& lhs, T rhs) noexcept
-	{
-		return UnitTypeLhs(lhs() / rhs);
-	}
-
-	/// Division of a scalar  by a unit_t type with a linear scale
-	template<class UnitTypeRhs, typename T,
-		std::enable_if_t<std::is_arithmetic<T>::value && traits::has_linear_scale<UnitTypeRhs>::value, int> = 0>
-		inline constexpr auto operator/(T lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>
-	{
-		using UnitsRhs = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		return unit_t<inverse<UnitsRhs>>
-			(lhs / rhs());
-	}
-
-	//----------------------------------
-	//	SCALAR COMPARISONS
-	//----------------------------------
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator==(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
-	{
-		return detail::abs(lhs - static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::epsilon() * detail::abs(lhs + static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) ||
-			detail::abs(lhs - static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs)) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min();
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator==(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
-	{
-		return detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) - rhs) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::epsilon() * detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) + rhs) ||
-			detail::abs(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) - rhs) < std::numeric_limits<UNIT_LIB_DEFAULT_TYPE>::min();
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator!=(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
-	{
-		return!(lhs == static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator!=(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
-	{
-		return !(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) == rhs);
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator>=(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
-	{
-		return std::isgreaterequal(lhs, static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator>=(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
-	{
-		return std::isgreaterequal(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs), rhs);
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator>(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
-	{
-		return lhs > static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs);
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator>(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
-	{
-		return static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) > rhs;
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator<=(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
-	{
-		return std::islessequal(lhs, static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs));
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator<=(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
-	{
-		return std::islessequal(static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs), rhs);
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator<(const UNIT_LIB_DEFAULT_TYPE lhs, const Units& rhs) noexcept
-	{
-		return lhs < static_cast<UNIT_LIB_DEFAULT_TYPE>(rhs);
-	}
-
-	template<typename Units, class = std::enable_if_t<units::traits::is_dimensionless_unit<Units>::value>>
-	constexpr bool operator<(const Units& lhs, const UNIT_LIB_DEFAULT_TYPE rhs) noexcept
-	{
-		return static_cast<UNIT_LIB_DEFAULT_TYPE>(lhs) < rhs;
-	}
-
-	//----------------------------------
-	//	POW
-	//----------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		/// recursive exponential implementation
-		template <int N, class U> struct power_of_unit
-		{
-			typedef typename units::detail::unit_multiply<U, typename power_of_unit<N - 1, U>::type> type;
-		};
-
-		/// End recursion
-		template <class U> struct power_of_unit<1, U>
-		{
-			typedef U type;
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace math
-	{
-		/**
-		 * @brief		computes the value of <i>value</i> raised to the <i>power</i>
-		 * @details		Only implemented for linear_scale units. <i>Power</i> must be known at compile time, so the resulting unit type can be deduced.
-		 * @tparam		power exponential power to raise <i>value</i> by.
-		 * @param[in]	value `unit_t` derived type to raise to the given <i>power</i>
-		 * @returns		new unit_t, raised to the given exponent
-		 */
-		template<int power, class UnitType, class = typename std::enable_if<traits::has_linear_scale<UnitType>::value, int>>
-		inline auto pow(const UnitType& value) noexcept -> unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
-		{
-			return unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
-				(std::pow(value(), power));
-		}
-
-		/**
-		 * @brief		computes the value of <i>value</i> raised to the <i>power</i> as a constexpr
-		 * @details		Only implemented for linear_scale units. <i>Power</i> must be known at compile time, so the resulting unit type can be deduced.
-		 *				Additionally, the power must be <i>a positive, integral, value</i>.
-		 * @tparam		power exponential power to raise <i>value</i> by.
-		 * @param[in]	value `unit_t` derived type to raise to the given <i>power</i>
-		 * @returns		new unit_t, raised to the given exponent
-		 */
-		template<int power, class UnitType, class = typename std::enable_if<traits::has_linear_scale<UnitType>::value, int>>
-		inline constexpr auto cpow(const UnitType& value) noexcept -> unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
-		{
-			static_assert(power >= 0, "cpow cannot accept negative numbers. Try units::math::pow instead.");
-			return unit_t<typename units::detail::power_of_unit<power, typename units::traits::unit_t_traits<UnitType>::unit_type>::type, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
-				(detail::pow(value(), power));
-		}
-	}
-
-	//------------------------------
-	//	DECIBEL SCALE
-	//------------------------------
-
-	/**
-	* @brief		unit_t scale for representing decibel values.
-	* @details		internally stores linearized values. `operator()` returns the value in dB.
-	* @tparam		T	underlying storage type
-	* @sa			unit_t
-	*/
-	template<typename T>
-	struct decibel_scale
-	{
-		inline constexpr decibel_scale() = default;
-		inline constexpr decibel_scale(const decibel_scale&) = default;
-		inline ~decibel_scale() = default;
-		inline decibel_scale& operator=(const decibel_scale&) = default;
-#if defined(_MSC_VER) && (_MSC_VER > 1800)
-		inline constexpr decibel_scale(decibel_scale&&) = default;
-		inline decibel_scale& operator=(decibel_scale&&) = default;
-#endif
-		inline constexpr decibel_scale(const T value) noexcept : m_value(std::pow(10, value / 10)) {}
-		template<class... Args>
-		inline constexpr decibel_scale(const T value, std::true_type, Args&&...) noexcept : m_value(value) {}
-		inline constexpr T operator()() const noexcept { return 10 * std::log10(m_value); }
-
-		T m_value;	///< linearized value	
-	};
-
-	//------------------------------
-	//	SCALAR (DECIBEL) UNITS
-	//------------------------------
-
-	/**
-	 * @brief		namespace for unit types and containers for units that have no dimension (scalar units)
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-	namespace dimensionless
-	{
-		typedef unit_t<scalar, UNIT_LIB_DEFAULT_TYPE, decibel_scale> dB_t;
-#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
-		inline std::ostream& operator<<(std::ostream& os, const dB_t& obj) { os << obj() << " dB"; return os; }
-#endif
-		typedef dB_t dBi_t;
-	}
-
-	//------------------------------
-	//	DECIBEL ARITHMETIC
-	//------------------------------
-
-	/// Addition for convertible unit_t types with a decibel_scale
-	template<class UnitTypeLhs, class UnitTypeRhs,
-		std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-	constexpr inline auto operator+(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<squared<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>>, typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type, decibel_scale>
-	{
-		using LhsUnits = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using RhsUnits = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
-
-		return unit_t<compound_unit<squared<LhsUnits>>, underlying_type, decibel_scale>
-			(lhs.template toLinearized<underlying_type>() * convert<RhsUnits, LhsUnits>(rhs.template toLinearized<underlying_type>()), std::true_type());
-	}
-
-	/// Addition between unit_t types with a decibel_scale and dimensionless dB units
-	template<class UnitTypeLhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value, int> = 0>
-	constexpr inline UnitTypeLhs operator+(const UnitTypeLhs& lhs, const dimensionless::dB_t& rhs) noexcept
-	{
-		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
-		return UnitTypeLhs(lhs.template toLinearized<underlying_type>() * rhs.template toLinearized<underlying_type>(), std::true_type());
-	}
-
-	/// Addition between unit_t types with a decibel_scale and dimensionless dB units
-	template<class UnitTypeRhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-	constexpr inline UnitTypeRhs operator+(const dimensionless::dB_t& lhs, const UnitTypeRhs& rhs) noexcept
-	{
-		using underlying_type = typename units::traits::unit_t_traits<UnitTypeRhs>::underlying_type;
-		return UnitTypeRhs(lhs.template toLinearized<underlying_type>() * rhs.template toLinearized<underlying_type>(), std::true_type());
-	}
-
-	/// Subtraction for convertible unit_t types with a decibel_scale
-	template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-	constexpr inline auto operator-(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<compound_unit<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type, inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>>, typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type, decibel_scale>
-	{
-		using LhsUnits = typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type;
-		using RhsUnits = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
-
-		return unit_t<compound_unit<LhsUnits, inverse<RhsUnits>>, underlying_type, decibel_scale>
-			(lhs.template toLinearized<underlying_type>() / convert<RhsUnits, LhsUnits>(rhs.template toLinearized<underlying_type>()), std::true_type());
-	}
-
-	/// Subtraction between unit_t types with a decibel_scale and dimensionless dB units
-	template<class UnitTypeLhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeLhs>::value && !traits::is_dimensionless_unit<UnitTypeLhs>::value, int> = 0>
-	constexpr inline UnitTypeLhs operator-(const UnitTypeLhs& lhs, const dimensionless::dB_t& rhs) noexcept
-	{
-		using underlying_type = typename units::traits::unit_t_traits<UnitTypeLhs>::underlying_type;
-		return UnitTypeLhs(lhs.template toLinearized<underlying_type>() / rhs.template toLinearized<underlying_type>(), std::true_type());
-	}
-
-	/// Subtraction between unit_t types with a decibel_scale and dimensionless dB units
-	template<class UnitTypeRhs, std::enable_if_t<traits::has_decibel_scale<UnitTypeRhs>::value && !traits::is_dimensionless_unit<UnitTypeRhs>::value, int> = 0>
-	constexpr inline auto operator-(const dimensionless::dB_t& lhs, const UnitTypeRhs& rhs) noexcept -> unit_t<inverse<typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type>, typename units::traits::unit_t_traits<UnitTypeRhs>::underlying_type, decibel_scale>
-	{
-		using RhsUnits = typename units::traits::unit_t_traits<UnitTypeRhs>::unit_type;
-		using underlying_type = typename units::traits::unit_t_traits<RhsUnits>::underlying_type;
-
-		return unit_t<inverse<RhsUnits>, underlying_type, decibel_scale>
-			(lhs.template toLinearized<underlying_type>() / rhs.template toLinearized<underlying_type>(), std::true_type());
-	}
-
-	//----------------------------------
-	//	UNIT RATIO CLASS
-	//----------------------------------
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		template<class Units>
-		struct _unit_value_t {};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	namespace traits
-	{
-#ifdef FOR_DOXYGEN_PURPOSES_ONLY
-		/**
-		* @ingroup		TypeTraits
-		* @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
-		* @details		The units library determines certain properties of the `unit_value_t` types passed to 
-		*				them and what they represent by using the members of the corresponding `unit_value_t_traits`
-		*				instantiation.
-		*/
-		template<typename T>
-		struct unit_value_t_traits
-		{
-			typedef typename T::unit_type unit_type;	///< Dimension represented by the `unit_value_t`.
-			typedef typename T::ratio ratio;			///< Quantity represented by the `unit_value_t`, expressed as arational number.
-		};
-#endif
-
-		/** @cond */	// DOXYGEN IGNORE
-		/**
-		 * @brief		unit_value_t_traits specialization for things which are not unit_t
-		 * @details
-		 */
-		template<typename T, typename = void>
-		struct unit_value_t_traits
-		{
-			typedef void unit_type;
-			typedef void ratio;
-		};
-	
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait for accessing the publically defined types of `units::unit_value_t_traits`
-		 * @details
-		 */
-		template<typename T>
-		struct unit_value_t_traits <T, typename void_t<
-			typename T::unit_type,
-			typename T::ratio>::type>
-		{
-			typedef typename T::unit_type unit_type;
-			typedef typename T::ratio ratio;
-		};
-		/** @endcond */	// END DOXYGEN IGNORE
-	}
-
-	//------------------------------------------------------------------------------
-	//	COMPILE-TIME UNIT VALUES AND ARITHMETIC
-	//------------------------------------------------------------------------------
-
-	/**
-	 * @ingroup		UnitContainers
-	 * @brief		Stores a rational unit value as a compile-time constant
-	 * @details		unit_value_t is useful for performing compile-time arithmetic on known 
-	 *				unit quantities.
-	 * @tparam		Units	units represented by the `unit_value_t`
-	 * @tparam		Num		numerator of the represented value.
-	 * @tparam		Denom	denominator of the represented value.
-	 * @sa			unit_value_t_traits to access information about the properties of the class,
-	 *				such as it's unit type and rational value.
-	 * @note		This is intentionally identical in concept to a `std::ratio`.
-	 *
-	 */
-	template<typename Units, std::uintmax_t Num, std::uintmax_t Denom = 1>
-	struct unit_value_t : units::detail::_unit_value_t<Units>
-	{
-		typedef Units unit_type;
-		typedef std::ratio<Num, Denom> ratio;
-
-		static_assert(traits::is_unit<Units>::value, "Template parameter `Units` must be a unit type.");
-		static constexpr const unit_t<Units> value() { return unit_t<Units>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den); }
-	};
-
-	namespace traits
-	{
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests whether a type is a unit_value_t representing the given unit type.
-		 * @details		e.g. `is_unit_value_t<meters, myType>::value` would test that `myType` is a 
-		 *				`unit_value_t<meters>`.
-		 * @tparam		Units	units that the `unit_value_t` is supposed to have.
-		 * @tparam		T		type to test.
-		 */
-		template<typename T, typename Units = typename traits::unit_value_t_traits<T>::unit_type>
-		struct is_unit_value_t : std::integral_constant<bool, 
-			std::is_base_of<units::detail::_unit_value_t<Units>, T>::value>
-		{};
-	
-		/**
-		 * @ingroup		TypeTraits
-		 * @brief		Trait which tests whether type T is a unit_value_t with a unit type in the given category.
-		 * @details		e.g. `is_unit_value_t_category<units::category::length, unit_value_t<feet>>::value` would be true
-		 */
-		template<typename Category, typename T>
-		struct is_unit_value_t_category : std::integral_constant<bool,
-			std::is_same<units::traits::base_unit_of<typename traits::unit_value_t_traits<T>::unit_type>, Category>::value>
-		{
-			static_assert(is_base_unit<Category>::value, "Template parameter `Category` must be a `base_unit` type.");
-		};
-	}
-
-	/** @cond */	// DOXYGEN IGNORE
-	namespace detail
-	{
-		// base class for common arithmetic
-		template<class U1, class U2>
-		struct unit_value_arithmetic
-		{
-			static_assert(traits::is_unit_value_t<U1>::value, "Template parameter `U1` must be a `unit_value_t` type.");
-			static_assert(traits::is_unit_value_t<U2>::value, "Template parameter `U2` must be a `unit_value_t` type.");
-
-			using _UNIT1 = typename traits::unit_value_t_traits<U1>::unit_type;
-			using _UNIT2 = typename traits::unit_value_t_traits<U2>::unit_type;
-			using _CONV1 = typename units::traits::unit_traits<_UNIT1>::conversion_ratio;
-			using _CONV2 = typename units::traits::unit_traits<_UNIT2>::conversion_ratio;
-			using _RATIO1 = typename traits::unit_value_t_traits<U1>::ratio;
-			using _RATIO2 = typename traits::unit_value_t_traits<U2>::ratio;
-			using _RATIO2CONV = typename std::ratio_divide<std::ratio_multiply<_RATIO2, _CONV2>, _CONV1>;
-			using _PI_EXP = std::ratio_subtract<typename units::traits::unit_traits<_UNIT2>::pi_exponent_ratio, typename units::traits::unit_traits<_UNIT1>::pi_exponent_ratio>;
-		};
-	}
-	/** @endcond */	// END DOXYGEN IGNORE
-
-	/**
-	 * @ingroup		CompileTimeUnitManipulators
-	 * @brief		adds two unit_value_t types at compile-time
-	 * @details		The resulting unit will the the `unit_type` of `U1`
-	 * @tparam		U1	left-hand `unit_value_t`
-	 * @tparam		U2	right-hand `unit_value_t`
-	 * @sa			unit_value_t_traits to access information about the properties of the class,
-	 *				such as it's unit type and rational value.
-	 * @note		very similar in concept to `std::ratio_add`
-	 */
-	template<class U1, class U2>
-	struct unit_value_add : units::detail::unit_value_arithmetic<U1, U2>, units::detail::_unit_value_t<typename traits::unit_value_t_traits<U1>::unit_type>
-	{
-		/** @cond */	// DOXYGEN IGNORE
-		using Base = units::detail::unit_value_arithmetic<U1, U2>;
-		typedef typename Base::_UNIT1 unit_type;
-		using ratio = std::ratio_add<typename Base::_RATIO1, typename Base::_RATIO2CONV>;
-
-		static_assert(traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, "Unit types are not compatible.");
-		/** @endcond */	// END DOXYGEN IGNORE
-
-		/**
-		 * @brief		Value of sum
-		 * @details		Returns the calculated value of the sum of `U1` and `U2`, in the same
-		 *				units as `U1`.
-		 * @returns		Value of the sum in the appropriate units.
-		 */
-		static constexpr const unit_t<unit_type> value() noexcept
-		{
-			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
-			return value(UsePi());
-		}
-
-		/** @cond */	// DOXYGEN IGNORE
-		// value if PI isn't involved
-		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
-		{ 
-			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
-		}
-
-		// value if PI *is* involved
-		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
-		{
-			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO1::num / Base::_RATIO1::den) +
-			((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO2CONV::num / Base::_RATIO2CONV::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
-		}
-		/** @endcond */	// END DOXYGEN IGNORE
-	};
-
-	/**
-	 * @ingroup		CompileTimeUnitManipulators
-	 * @brief		subtracts two unit_value_t types at compile-time
-	 * @details		The resulting unit will the the `unit_type` of `U1`
-	 * @tparam		U1	left-hand `unit_value_t`
-	 * @tparam		U2	right-hand `unit_value_t`
-	 * @sa			unit_value_t_traits to access information about the properties of the class,
-	 *				such as it's unit type and rational value.
-	 * @note		very similar in concept to `std::ratio_subtract`
-	 */
-	template<class U1, class U2>
-	struct unit_value_subtract : units::detail::unit_value_arithmetic<U1, U2>, units::detail::_unit_value_t<typename traits::unit_value_t_traits<U1>::unit_type>
-	{
-		/** @cond */	// DOXYGEN IGNORE
-		using Base = units::detail::unit_value_arithmetic<U1, U2>;
-
-		typedef typename Base::_UNIT1 unit_type;
-		using ratio = std::ratio_subtract<typename Base::_RATIO1, typename Base::_RATIO2CONV>;
-
-		static_assert(traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, "Unit types are not compatible.");
-		/** @endcond */	// END DOXYGEN IGNORE
-
-		/**
-		 * @brief		Value of difference
-		 * @details		Returns the calculated value of the difference of `U1` and `U2`, in the same
-		 *				units as `U1`.
-		 * @returns		Value of the difference in the appropriate units.
-		 */
-		static constexpr const unit_t<unit_type> value() noexcept
-		{
-			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
-			return value(UsePi());
-		}
-
-		/** @cond */	// DOXYGEN IGNORE
-		// value if PI isn't involved
-		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
-		{
-			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
-		}
-
-		// value if PI *is* involved
-		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
-		{
-			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO1::num / Base::_RATIO1::den) - ((UNIT_LIB_DEFAULT_TYPE)Base::_RATIO2CONV::num / Base::_RATIO2CONV::den)
-				* std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
-		}
-		/** @endcond */	// END DOXYGEN IGNORE	};
-	};
-
-	/**
-	 * @ingroup		CompileTimeUnitManipulators
-	 * @brief		multiplies two unit_value_t types at compile-time
-	 * @details		The resulting unit will the the `unit_type` of `U1 * U2`
-	 * @tparam		U1	left-hand `unit_value_t`
-	 * @tparam		U2	right-hand `unit_value_t`
-	 * @sa			unit_value_t_traits to access information about the properties of the class,
-	 *				such as it's unit type and rational value.
-	 * @note		very similar in concept to `std::ratio_multiply`
-	 */
-	template<class U1, class U2>
-	struct unit_value_multiply : units::detail::unit_value_arithmetic<U1, U2>,
-		units::detail::_unit_value_t<typename std::conditional<traits::is_convertible_unit<typename traits::unit_value_t_traits<U1>::unit_type,
-			typename traits::unit_value_t_traits<U2>::unit_type>::value, compound_unit<squared<typename traits::unit_value_t_traits<U1>::unit_type>>, 
-			compound_unit<typename traits::unit_value_t_traits<U1>::unit_type, typename traits::unit_value_t_traits<U2>::unit_type>>::type>
-	{
-		/** @cond */	// DOXYGEN IGNORE
-		using Base = units::detail::unit_value_arithmetic<U1, U2>;
-		
-		using unit_type = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, compound_unit<squared<typename Base::_UNIT1>>, compound_unit<typename Base::_UNIT1, typename Base::_UNIT2>>;
-		using ratio = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, std::ratio_multiply<typename Base::_RATIO1, typename Base::_RATIO2CONV>, std::ratio_multiply<typename Base::_RATIO1, typename Base::_RATIO2>>;
-		/** @endcond */	// END DOXYGEN IGNORE
-
-		/**
-		 * @brief		Value of product
-		 * @details		Returns the calculated value of the product of `U1` and `U2`, in units
-		 *				of `U1 x U2`.
-		 * @returns		Value of the product in the appropriate units.
-		 */
-		static constexpr const unit_t<unit_type> value() noexcept
-		{
-			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
-			return value(UsePi());
-		}
-
-		/** @cond */	// DOXYGEN IGNORE
-		// value if PI isn't involved
-		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
-		{
-			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
-		}
-
-		// value if PI *is* involved
-		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
-		{
-			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
-		}
-		/** @endcond */	// END DOXYGEN IGNORE
-	};
-
-	/**
-	 * @ingroup		CompileTimeUnitManipulators
-	 * @brief		divides two unit_value_t types at compile-time
-	 * @details		The resulting unit will the the `unit_type` of `U1`
-	 * @tparam		U1	left-hand `unit_value_t`
-	 * @tparam		U2	right-hand `unit_value_t`
-	 * @sa			unit_value_t_traits to access information about the properties of the class,
-	 *				such as it's unit type and rational value.
-	 * @note		very similar in concept to `std::ratio_divide`
-	 */
-	template<class U1, class U2>
-	struct unit_value_divide : units::detail::unit_value_arithmetic<U1, U2>,
-		units::detail::_unit_value_t<typename std::conditional<traits::is_convertible_unit<typename traits::unit_value_t_traits<U1>::unit_type,
-		typename traits::unit_value_t_traits<U2>::unit_type>::value, dimensionless::scalar, compound_unit<typename traits::unit_value_t_traits<U1>::unit_type, 
-		inverse<typename traits::unit_value_t_traits<U2>::unit_type>>>::type>
-	{
-		/** @cond */	// DOXYGEN IGNORE
-		using Base = units::detail::unit_value_arithmetic<U1, U2>;
-		
-		using unit_type = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, dimensionless::scalar, compound_unit<typename Base::_UNIT1, inverse<typename Base::_UNIT2>>>;
-		using ratio = std::conditional_t<traits::is_convertible_unit<typename Base::_UNIT1, typename Base::_UNIT2>::value, std::ratio_divide<typename Base::_RATIO1, typename Base::_RATIO2CONV>, std::ratio_divide<typename Base::_RATIO1, typename Base::_RATIO2>>;
-		/** @endcond */	// END DOXYGEN IGNORE
-
-		/**
-		 * @brief		Value of quotient
-		 * @details		Returns the calculated value of the quotient of `U1` and `U2`, in units
-		 *				of `U1 x U2`.
-		 * @returns		Value of the quotient in the appropriate units.
-		 */
-		static constexpr const unit_t<unit_type> value() noexcept
-		{
-			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
-			return value(UsePi());
-		}
-
-		/** @cond */	// DOXYGEN IGNORE
-		// value if PI isn't involved
-		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
-		{
-			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
-		}
-
-		// value if PI *is* involved
-		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
-		{
-			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)Base::_PI_EXP::num / Base::_PI_EXP::den)));
-		}
-		/** @endcond */	// END DOXYGEN IGNORE
-	};
-
-	/**
-	 * @ingroup		CompileTimeUnitManipulators
-	 * @brief		raises unit_value_to a power at compile-time
-	 * @details		The resulting unit will the `unit_type` of `U1` squared
-	 * @tparam		U1	`unit_value_t` to take the exponentiation of.
-	 * @sa			unit_value_t_traits to access information about the properties of the class,
-	 *				such as it's unit type and rational value.
-	 * @note		very similar in concept to `units::math::pow`
-	 */
-	template<class U1, int power>
-	struct unit_value_power : units::detail::unit_value_arithmetic<U1, U1>, units::detail::_unit_value_t<typename units::detail::power_of_unit<power, typename traits::unit_value_t_traits<U1>::unit_type>::type>
-	{
-		/** @cond */	// DOXYGEN IGNORE
-		using Base = units::detail::unit_value_arithmetic<U1, U1>;
-
-		using unit_type = typename units::detail::power_of_unit<power, typename Base::_UNIT1>::type;
-		using ratio = typename units::detail::power_of_ratio<power, typename Base::_RATIO1>::type;
-		using pi_exponent = std::ratio_multiply<std::ratio<power>, typename Base::_UNIT1::pi_exponent_ratio>;
-		/** @endcond */	// END DOXYGEN IGNORE
-
-		/**
-		 * @brief		Value of exponentiation
-		 * @details		Returns the calculated value of the exponentiation of `U1`, in units
-		 *				of `U1^power`.
-		 * @returns		Value of the exponentiation in the appropriate units.
-		 */
-		static constexpr const unit_t<unit_type> value() noexcept
-		{
-			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
-			return value(UsePi());
-		}
-
-		/** @cond */	// DOXYGEN IGNORE
-		// value if PI isn't involved
-		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
-		{
-			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
-		}
-
-		// value if PI *is* involved
-		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
-		{
-			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)pi_exponent::num / pi_exponent::den)));
-		}
-		/** @endcond */	// END DOXYGEN IGNORE	};
-	};
-
-	/**
-	 * @ingroup		CompileTimeUnitManipulators
-	 * @brief		calculates square root of unit_value_t at compile-time
-	 * @details		The resulting unit will the square root `unit_type` of `U1`	 
-	 * @tparam		U1	`unit_value_t` to take the square root of.
-	 * @sa			unit_value_t_traits to access information about the properties of the class,
-	 *				such as it's unit type and rational value.
-	 * @note		very similar in concept to `units::ratio_sqrt`
-	 */
-	template<class U1, std::intmax_t Eps = 10000000000>
-	struct unit_value_sqrt : units::detail::unit_value_arithmetic<U1, U1>, units::detail::_unit_value_t<square_root<typename traits::unit_value_t_traits<U1>::unit_type, Eps>>
-	{
-		/** @cond */	// DOXYGEN IGNORE
-		using Base = units::detail::unit_value_arithmetic<U1, U1>;
-
-		using unit_type = square_root<typename Base::_UNIT1, Eps>;
-		using ratio = ratio_sqrt<typename Base::_RATIO1, Eps>;
-		using pi_exponent = ratio_sqrt<typename Base::_UNIT1::pi_exponent_ratio, Eps>;
-		/** @endcond */	// END DOXYGEN IGNORE
-
-		/**
-		 * @brief		Value of square root
-		 * @details		Returns the calculated value of the square root of `U1`, in units
-		 *				of `U1^1/2`.
-		 * @returns		Value of the square root in the appropriate units.
-		 */
-		static constexpr const unit_t<unit_type> value() noexcept
-		{
-			using UsePi = std::integral_constant<bool, Base::_PI_EXP::num != 0>;
-			return value(UsePi());
-		}
-
-		/** @cond */	// DOXYGEN IGNORE
-		// value if PI isn't involved
-		static constexpr const unit_t<unit_type> value(std::false_type) noexcept
-		{
-			return unit_t<unit_type>((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den);
-		}
-
-		// value if PI *is* involved
-		static constexpr const unit_t<unit_type> value(std::true_type) noexcept
-		{
-			return unit_t<unit_type>(((UNIT_LIB_DEFAULT_TYPE)ratio::num / ratio::den) * std::pow(units::constants::detail::PI_VAL, ((UNIT_LIB_DEFAULT_TYPE)pi_exponent::num / pi_exponent::den)));
-		}
-		/** @endcond */	// END DOXYGEN IGNORE
-	};
-
-	//------------------------------
-	//	LITERALS
-	//------------------------------
-
-	/**
-	 * @namespace	units::literals
-	 * @brief		namespace for unit literal definitions of all categories.
-	 * @details		Literals allow for declaring unit types using suffix values. For example, a type
-	 *				of `meter_t(6.2)` could be declared as `6.2_m`. All literals use an underscore
-	 *				followed by the abbreviation for the unit. To enable literal syntax in your code,
-	 *				include the statement `using namespace units::literals`.
-	 * @anchor		unitLiterals
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-
-	//------------------------------
-	//	LENGTH UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::length
-	 * @brief		namespace for unit types and containers representing length values
-	 * @details		The SI unit for length is `meters`, and the corresponding `base_unit` category is
-	 *				`length_unit`.
-	 * @anchor		lengthContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_LENGTH_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(length, meter, meters, m, unit<std::ratio<1>, units::category::length_unit>)
-	UNIT_ADD(length, foot, feet, ft, unit<std::ratio<381, 1250>, meters>)
-	UNIT_ADD(length, mil, mils, mil, unit<std::ratio<1000>, feet>)
-	UNIT_ADD(length, inch, inches, in, unit<std::ratio<1, 12>, feet>)
-	UNIT_ADD(length, mile,   miles,    mi,    unit<std::ratio<5280>, feet>)
-	UNIT_ADD(length, nauticalMile, nauticalMiles, nmi, unit<std::ratio<1852>, meters>)
-	UNIT_ADD(length, astronicalUnit, astronicalUnits, au, unit<std::ratio<149597870700>, meters>)
-	UNIT_ADD(length, lightyear, lightyears, ly, unit<std::ratio<9460730472580800>, meters>)
-	UNIT_ADD(length, parsec, parsecs, pc, unit<std::ratio<648000>, astronicalUnits, std::ratio<-1>>)
-	UNIT_ADD(length, angstrom, angstroms, angstrom, unit<std::ratio<1, 10>, nanometers>)
-	UNIT_ADD(length, cubit, cubits, cbt, unit<std::ratio<18>, inches>)
-	UNIT_ADD(length, fathom, fathoms, ftm, unit<std::ratio<6>, feet>)
-	UNIT_ADD(length, chain, chains, ch, unit<std::ratio<66>, feet>)
-	UNIT_ADD(length, furlong, furlongs, fur, unit<std::ratio<10>, chains>)
-	UNIT_ADD(length, hand, hands, hand, unit<std::ratio<4>, inches>)
-	UNIT_ADD(length, league, leagues, lea, unit<std::ratio<3>, miles>)
-	UNIT_ADD(length, nauticalLeague, nauticalLeagues, nl, unit<std::ratio<3>, nauticalMiles>)
-	UNIT_ADD(length, yard, yards, yd, unit<std::ratio<3>, feet>)
-
-	UNIT_ADD_CATEGORY_TRAIT(length)
-#endif
-
-	//------------------------------
-	//	MASS UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::mass
-	 * @brief		namespace for unit types and containers representing mass values
-	 * @details		The SI unit for mass is `kilograms`, and the corresponding `base_unit` category is
-	 *				`mass_unit`.
-	 * @anchor		massContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_MASS_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(mass, gram, grams, g, unit<std::ratio<1, 1000>, units::category::mass_unit>)
-	UNIT_ADD(mass, metric_ton, metric_tons, t, unit<std::ratio<1000>, kilograms>)
-	UNIT_ADD(mass, pound, pounds, lb, unit<std::ratio<45359237, 100000000>, kilograms>)
-	UNIT_ADD(mass, long_ton, long_tons, ln_t, unit<std::ratio<2240>, pounds>)
-	UNIT_ADD(mass, short_ton, short_tons, sh_t, unit<std::ratio<2000>, pounds>)
-	UNIT_ADD(mass, stone, stone, st, unit<std::ratio<14>, pounds>)
-	UNIT_ADD(mass, ounce, ounces, oz, unit<std::ratio<1, 16>, pounds>)
-	UNIT_ADD(mass, carat, carats, ct, unit<std::ratio<200>, milligrams>)
-	UNIT_ADD(mass, slug, slugs, slug, unit<std::ratio<145939029, 10000000>, kilograms>)
-
-	UNIT_ADD_CATEGORY_TRAIT(mass)
-#endif
-
-	//------------------------------
-	//	TIME UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::time
-	 * @brief		namespace for unit types and containers representing time values
-	 * @details		The SI unit for time is `seconds`, and the corresponding `base_unit` category is
-	 *				`time_unit`.
-	 * @anchor		timeContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_TIME_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(time, second, seconds, s, unit<std::ratio<1>, units::category::time_unit>)
-	UNIT_ADD(time, minute, minutes, min, unit<std::ratio<60>, seconds>)
-	UNIT_ADD(time, hour, hours, hr, unit<std::ratio<60>, minutes>)
-	UNIT_ADD(time, day, days, d, unit<std::ratio<24>, hours>)
-	UNIT_ADD(time, week, weeks, wk, unit<std::ratio<7>, days>)
-	UNIT_ADD(time, year, years, yr, unit<std::ratio<365>, days>)
-	UNIT_ADD(time, julian_year, julian_years, a_j,	unit<std::ratio<31557600>, seconds>)
-	UNIT_ADD(time, gregorian_year, gregorian_years, a_g, unit<std::ratio<31556952>, seconds>)
-
-	UNIT_ADD_CATEGORY_TRAIT(time)
-#endif
-
-	//------------------------------
-	//	ANGLE UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::angle
-	 * @brief		namespace for unit types and containers representing angle values
-	 * @details		The SI unit for angle is `radians`, and the corresponding `base_unit` category is
-	 *				`angle_unit`.
-	 * @anchor		angleContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(angle, radian, radians, rad, unit<std::ratio<1>, units::category::angle_unit>)
-	UNIT_ADD(angle, degree, degrees, deg, unit<std::ratio<1, 180>, radians, std::ratio<1>>)
-	UNIT_ADD(angle, arcminute, arcminutes, arcmin, unit<std::ratio<1, 60>, degrees>)
-	UNIT_ADD(angle, arcsecond, arcseconds, arcsec, unit<std::ratio<1, 60>, arcminutes>)
-	UNIT_ADD(angle, milliarcsecond, milliarcseconds, mas, milli<arcseconds>)
-	UNIT_ADD(angle, turn, turns, tr, unit<std::ratio<2>, radians, std::ratio<1>>)
-	UNIT_ADD(angle, gradian, gradians, gon, unit<std::ratio<1, 400>, turns>)
-
-	UNIT_ADD_CATEGORY_TRAIT(angle)
-#endif
-
-	//------------------------------
-	//	UNITS OF CURRENT
-	//------------------------------
-	/**
-	 * @namespace	units::current
-	 * @brief		namespace for unit types and containers representing current values
-	 * @details		The SI unit for current is `amperes`, and the corresponding `base_unit` category is
-	 *				`current_unit`.
-	 * @anchor		currentContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_CURRENT_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(current, ampere, amperes, A, unit<std::ratio<1>, units::category::current_unit>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(current)
-#endif
-
-	//------------------------------
-	//	UNITS OF TEMPERATURE
-	//------------------------------
-
-	// NOTE: temperature units have special conversion overloads, since they
-	// require translations and aren't a reversible transform.
-
-	/**
-	 * @namespace	units::temperature
-	 * @brief		namespace for unit types and containers representing temperature values
-	 * @details		The SI unit for temperature is `kelvin`, and the corresponding `base_unit` category is
-	 *				`temperature_unit`.
-	 * @anchor		temperatureContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_TEMPERATURE_UNITS)
-	UNIT_ADD(temperature, kelvin, kelvin, K, unit<std::ratio<1>, units::category::temperature_unit>)
-	UNIT_ADD(temperature, celsius, celsius, degC, unit<std::ratio<1>, kelvin, std::ratio<0>, std::ratio<27315, 100>>)
-	UNIT_ADD(temperature, fahrenheit, fahrenheit, degF, unit<std::ratio<5, 9>, celsius, std::ratio<0>, std::ratio<-160, 9>>)
-	UNIT_ADD(temperature, reaumur, reaumur, Re, unit<std::ratio<10, 8>, celsius>)
-	UNIT_ADD(temperature, rankine, rankine, Ra, unit<std::ratio<5, 9>, kelvin>)
-
-	UNIT_ADD_CATEGORY_TRAIT(temperature)
-#endif
-
-	//------------------------------
-	//	UNITS OF AMOUNT OF SUBSTANCE
-	//------------------------------
-
-	/**
-	 * @namespace	units::substance
-	 * @brief		namespace for unit types and containers representing substance values
-	 * @details		The SI unit for substance is `moles`, and the corresponding `base_unit` category is
-	 *				`substance_unit`.
-	 * @anchor		substanceContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_SUBSTANCE_UNITS)
-	UNIT_ADD(substance, mole, moles, mol, unit<std::ratio<1>, units::category::substance_unit>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(substance)
-#endif
-
-	//------------------------------
-	//	UNITS OF LUMINOUS INTENSITY
-	//------------------------------
-
-	/**
-	 * @namespace	units::luminous_intensity
-	 * @brief		namespace for unit types and containers representing luminous_intensity values
-	 * @details		The SI unit for luminous_intensity is `candelas`, and the corresponding `base_unit` category is
-	 *				`luminous_intensity_unit`.
-	 * @anchor		luminousIntensityContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_LUMINOUS_INTENSITY_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(luminous_intensity, candela, candelas, cd, unit<std::ratio<1>, units::category::luminous_intensity_unit>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(luminous_intensity)
-#endif
-
-	//------------------------------
-	//	UNITS OF SOLID ANGLE
-	//------------------------------
-
-	/**
-	 * @namespace	units::solid_angle
-	 * @brief		namespace for unit types and containers representing solid_angle values
-	 * @details		The SI unit for solid_angle is `steradians`, and the corresponding `base_unit` category is
-	 *				`solid_angle_unit`.
-	 * @anchor		solidAngleContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_SOLID_ANGLE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(solid_angle, steradian, steradians, sr, unit<std::ratio<1>, units::category::solid_angle_unit>)
-	UNIT_ADD(solid_angle, degree_squared, degrees_squared, sq_deg, squared<angle::degrees>)
-	UNIT_ADD(solid_angle, spat, spats, sp, unit<std::ratio<4>, steradians, std::ratio<1>>)
-
-	UNIT_ADD_CATEGORY_TRAIT(solid_angle)
-#endif
-
-	//------------------------------
-	//	FREQUENCY UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::frequency
-	 * @brief		namespace for unit types and containers representing frequency values
-	 * @details		The SI unit for frequency is `hertz`, and the corresponding `base_unit` category is
-	 *				`frequency_unit`.
-	 * @anchor		frequencyContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_FREQUENCY_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(frequency, hertz, hertz, Hz, unit<std::ratio<1>, units::category::frequency_unit>)
-
-	UNIT_ADD_CATEGORY_TRAIT(frequency)
-#endif
-
-	//------------------------------
-	//	VELOCITY UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::velocity
-	 * @brief		namespace for unit types and containers representing velocity values
-	 * @details		The SI unit for velocity is `meters_per_second`, and the corresponding `base_unit` category is
-	 *				`velocity_unit`.
-	 * @anchor		velocityContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_VELOCITY_UNITS)
-	UNIT_ADD(velocity, meters_per_second, meters_per_second, mps, unit<std::ratio<1>, units::category::velocity_unit>)
-	UNIT_ADD(velocity, feet_per_second, feet_per_second, fps, compound_unit<length::feet, inverse<time::seconds>>)
-	UNIT_ADD(velocity, miles_per_hour, miles_per_hour, mph, compound_unit<length::miles, inverse<time::hour>>)
-	UNIT_ADD(velocity, kilometers_per_hour, kilometers_per_hour, kph, compound_unit<length::kilometers, inverse<time::hour>>)
-	UNIT_ADD(velocity, knot, knots, kts, compound_unit<length::nauticalMiles, inverse<time::hour>>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(velocity)
-#endif
-
-	//------------------------------
-	//	ANGULAR VELOCITY UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::angular_velocity
-	 * @brief		namespace for unit types and containers representing angular velocity values
-	 * @details		The SI unit for angular velocity is `radians_per_second`, and the corresponding `base_unit` category is
-	 *				`angular_velocity_unit`.
-	 * @anchor		angularVelocityContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGULAR_VELOCITY_UNITS)
-	UNIT_ADD(angular_velocity, radians_per_second, radians_per_second, rad_per_s, 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, 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, mas_per_yr, compound_unit<angle::milliarcseconds, inverse<time::year>>)
-
-	UNIT_ADD_CATEGORY_TRAIT(angular_velocity)
-#endif
-
-	//------------------------------
-	//	UNITS OF ACCELERATION
-	//------------------------------
-
-	/**
-	 * @namespace	units::acceleration
-	 * @brief		namespace for unit types and containers representing acceleration values
-	 * @details		The SI unit for acceleration is `meters_per_second_squared`, and the corresponding `base_unit` category is
-	 *				`acceleration_unit`.
-	 * @anchor		accelerationContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ACCELERATION_UNITS)
-	UNIT_ADD(acceleration, meters_per_second_squared, meters_per_second_squared, mps_sq, unit<std::ratio<1>, units::category::acceleration_unit>)
-	UNIT_ADD(acceleration, feet_per_second_squared, feet_per_second_squared, fps_sq, compound_unit<length::feet, inverse<squared<time::seconds>>>)
-	UNIT_ADD(acceleration, standard_gravity, standard_gravity, SG, unit<std::ratio<980665, 100000>, meters_per_second_squared>)
-
-	UNIT_ADD_CATEGORY_TRAIT(acceleration)
-#endif
-
-	//------------------------------
-	//	UNITS OF FORCE
-	//------------------------------
-
-	/**
-	 * @namespace	units::force
-	 * @brief		namespace for unit types and containers representing force values
-	 * @details		The SI unit for force is `newtons`, and the corresponding `base_unit` category is
-	 *				`force_unit`.
-	 * @anchor		forceContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_FORCE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(force, newton, newtons, N, unit<std::ratio<1>, units::category::force_unit>)
-	UNIT_ADD(force, pound, pounds, lbf, compound_unit<mass::slug, length::foot, inverse<squared<time::seconds>>>)
-	UNIT_ADD(force, dyne, dynes, dyn, unit<std::ratio<1, 100000>, newtons>)
-	UNIT_ADD(force, kilopond, kiloponds, kp, compound_unit<acceleration::standard_gravity, mass::kilograms>)
-	UNIT_ADD(force, poundal, poundals, pdl, compound_unit<mass::pound, length::foot, inverse<squared<time::seconds>>>)
-
-	UNIT_ADD_CATEGORY_TRAIT(force)
-#endif
-
-	//------------------------------
-	//	UNITS OF PRESSURE
-	//------------------------------
-
-	/**
-	 * @namespace	units::pressure
-	 * @brief		namespace for unit types and containers representing pressure values
-	 * @details		The SI unit for pressure is `pascals`, and the corresponding `base_unit` category is
-	 *				`pressure_unit`.
-	 * @anchor		pressureContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_PRESSURE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(pressure, pascal, pascals, Pa, unit<std::ratio<1>, units::category::pressure_unit>)
-	UNIT_ADD(pressure, bar, bars, bar, unit<std::ratio<100>, kilo<pascals>>)
-	UNIT_ADD(pressure, mbar, mbars, mbar, unit<std::ratio<1>, milli<bar>>)
-	UNIT_ADD(pressure, atmosphere, atmospheres, atm, unit<std::ratio<101325>, pascals>)
-	UNIT_ADD(pressure, pounds_per_square_inch, pounds_per_square_inch, psi, compound_unit<force::pounds, inverse<squared<length::inch>>>)
-	UNIT_ADD(pressure, torr, torrs, torr, unit<std::ratio<1, 760>, atmospheres>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(pressure)
-#endif
-
-	//------------------------------
-	//	UNITS OF CHARGE
-	//------------------------------
-
-	/**
-	 * @namespace	units::charge
-	 * @brief		namespace for unit types and containers representing charge values
-	 * @details		The SI unit for charge is `coulombs`, and the corresponding `base_unit` category is
-	 *				`charge_unit`.
-	 * @anchor		chargeContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_CHARGE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(charge, coulomb, coulombs, C, unit<std::ratio<1>, units::category::charge_unit>)
-	UNIT_ADD_WITH_METRIC_PREFIXES(charge, ampere_hour, ampere_hours, Ah, compound_unit<current::ampere, time::hours>)
-
-	UNIT_ADD_CATEGORY_TRAIT(charge)
-#endif
-
-	//------------------------------
-	//	UNITS OF ENERGY
-	//------------------------------
-
-	/**
-	 * @namespace	units::energy
-	 * @brief		namespace for unit types and containers representing energy values
-	 * @details		The SI unit for energy is `joules`, and the corresponding `base_unit` category is
-	 *				`energy_unit`.
-	 * @anchor		energyContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ENERGY_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(energy, joule, joules, J, unit<std::ratio<1>, units::category::energy_unit>)
-	UNIT_ADD_WITH_METRIC_PREFIXES(energy, calorie, calories, cal, unit<std::ratio<4184, 1000>, joules>)
-	UNIT_ADD(energy, kilowatt_hour, kilowatt_hours, kWh, unit<std::ratio<36, 10>, megajoules>)
-	UNIT_ADD(energy, watt_hour, watt_hours, Wh, unit<std::ratio<1, 1000>, kilowatt_hours>)
-	UNIT_ADD(energy, british_thermal_unit, british_thermal_units, BTU, unit<std::ratio<105505585262, 100000000>, joules>)
-	UNIT_ADD(energy, british_thermal_unit_iso, british_thermal_units_iso, BTU_iso, unit<std::ratio<1055056, 1000>, joules>)
-	UNIT_ADD(energy, british_thermal_unit_59, british_thermal_units_59, BTU59, unit<std::ratio<1054804, 1000>, joules>)
-	UNIT_ADD(energy, therm, therms, thm, unit<std::ratio<100000>, british_thermal_units_59>)
-	UNIT_ADD(energy, foot_pound, foot_pounds, ftlbf, unit<std::ratio<13558179483314004, 10000000000000000>, joules>)
-
-	UNIT_ADD_CATEGORY_TRAIT(energy)
-#endif
-
-	//------------------------------
-	//	UNITS OF POWER
-	//------------------------------
-
-	/**
-	 * @namespace	units::power
-	 * @brief		namespace for unit types and containers representing power values
-	 * @details		The SI unit for power is `watts`, and the corresponding `base_unit` category is
-	 *				`power_unit`.
-	 * @anchor		powerContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_POWER_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(power, watt, watts, W, unit<std::ratio<1>, units::category::power_unit>)
-	UNIT_ADD(power, horsepower, horsepower, hp, unit<std::ratio<7457, 10>, watts>)
-	UNIT_ADD_DECIBEL(power, watt, dBW)
-	UNIT_ADD_DECIBEL(power, milliwatt, dBm)
-	
-	UNIT_ADD_CATEGORY_TRAIT(power)
-#endif
-
-	//------------------------------
-	//	UNITS OF VOLTAGE
-	//------------------------------
-
-	/**
-	 * @namespace	units::voltage
-	 * @brief		namespace for unit types and containers representing voltage values
-	 * @details		The SI unit for voltage is `volts`, and the corresponding `base_unit` category is
-	 *				`voltage_unit`.
-	 * @anchor		voltageContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_VOLTAGE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(voltage, volt, volts, V, unit<std::ratio<1>, units::category::voltage_unit>)
-	UNIT_ADD(voltage, statvolt, statvolts, statV, unit<std::ratio<1000000, 299792458>, volts>)
-	UNIT_ADD(voltage, abvolt, abvolts, abV, unit<std::ratio<1, 100000000>, volts>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(voltage)
-#endif
-
-	//------------------------------
-	//	UNITS OF CAPACITANCE
-	//------------------------------
-
-	/**
-	 * @namespace	units::capacitance
-	 * @brief		namespace for unit types and containers representing capacitance values
-	 * @details		The SI unit for capacitance is `farads`, and the corresponding `base_unit` category is
-	 *				`capacitance_unit`.
-	 * @anchor		capacitanceContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_CAPACITANCE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(capacitance, farad, farads, F, unit<std::ratio<1>, units::category::capacitance_unit>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(capacitance)
-#endif
-
-	//------------------------------
-	//	UNITS OF IMPEDANCE
-	//------------------------------
-
-	/**
-	 * @namespace	units::impedance
-	 * @brief		namespace for unit types and containers representing impedance values
-	 * @details		The SI unit for impedance is `ohms`, and the corresponding `base_unit` category is
-	 *				`impedance_unit`.
-	 * @anchor		impedanceContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_IMPEDANCE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(impedance, ohm, ohms, Ohm, unit<std::ratio<1>, units::category::impedance_unit>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(impedance)
-#endif
-
-	//------------------------------
-	//	UNITS OF CONDUCTANCE
-	//------------------------------
-
-	/**
-	 * @namespace	units::conductance
-	 * @brief		namespace for unit types and containers representing conductance values
-	 * @details		The SI unit for conductance is `siemens`, and the corresponding `base_unit` category is
-	 *				`conductance_unit`.
-	 * @anchor		conductanceContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_CONDUCTANCE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(conductance, siemens, siemens, S, unit<std::ratio<1>, units::category::conductance_unit>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(conductance)
-#endif
-
-	//------------------------------
-	//	UNITS OF MAGNETIC FLUX
-	//------------------------------
-
-	/**
-	 * @namespace	units::magnetic_flux
-	 * @brief		namespace for unit types and containers representing magnetic_flux values
-	 * @details		The SI unit for magnetic_flux is `webers`, and the corresponding `base_unit` category is
-	 *				`magnetic_flux_unit`.
-	 * @anchor		magneticFluxContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_MAGNETIC_FLUX_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(magnetic_flux, weber, webers, Wb, unit<std::ratio<1>, units::category::magnetic_flux_unit>)
-	UNIT_ADD(magnetic_flux, maxwell, maxwells, Mx, unit<std::ratio<1, 100000000>, webers>)
-
-	UNIT_ADD_CATEGORY_TRAIT(magnetic_flux)
-#endif
-
-	//----------------------------------------
-	//	UNITS OF MAGNETIC FIELD STRENGTH
-	//----------------------------------------
-
-	/**
-	 * @namespace	units::magnetic_field_strength
-	 * @brief		namespace for unit types and containers representing magnetic_field_strength values
-	 * @details		The SI unit for magnetic_field_strength is `teslas`, and the corresponding `base_unit` category is
-	 *				`magnetic_field_strength_unit`.
-	 * @anchor		magneticFieldStrengthContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-	// Unfortunately `_T` is a WINAPI macro, so we have to use `_Te` as the tesla abbreviation.
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_MAGNETIC_FIELD_STRENGTH_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(magnetic_field_strength, tesla, teslas, Te, unit<std::ratio<1>, units::category::magnetic_field_strength_unit>)
-	UNIT_ADD(magnetic_field_strength, gauss, gauss, G, compound_unit<magnetic_flux::maxwell, inverse<squared<length::centimeter>>>)
-		
-	UNIT_ADD_CATEGORY_TRAIT(magnetic_field_strength)
-#endif
-
-	//------------------------------
-	//	UNITS OF INDUCTANCE
-	//------------------------------
-
-	/**
-	 * @namespace	units::inductance
-	 * @brief		namespace for unit types and containers representing inductance values
-	 * @details		The SI unit for inductance is `henrys`, and the corresponding `base_unit` category is
-	 *				`inductance_unit`.
-	 * @anchor		inductanceContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_INDUCTANCE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(inductance, henry, henries, H, unit<std::ratio<1>, units::category::inductance_unit>)
-
-	UNIT_ADD_CATEGORY_TRAIT(inductance)
-#endif
-
-	//------------------------------
-	//	UNITS OF LUMINOUS FLUX
-	//------------------------------
-
-	/**
-	 * @namespace	units::luminous_flux
-	 * @brief		namespace for unit types and containers representing luminous_flux values
-	 * @details		The SI unit for luminous_flux is `lumens`, and the corresponding `base_unit` category is
-	 *				`luminous_flux_unit`.
-	 * @anchor		luminousFluxContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_LUMINOUS_FLUX_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(luminous_flux, lumen, lumens, lm, unit<std::ratio<1>, units::category::luminous_flux_unit>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(luminous_flux)
-#endif
-
-	//------------------------------
-	//	UNITS OF ILLUMINANCE
-	//------------------------------
-
-	/**
-	 * @namespace	units::illuminance
-	 * @brief		namespace for unit types and containers representing illuminance values
-	 * @details		The SI unit for illuminance is `luxes`, and the corresponding `base_unit` category is
-	 *				`illuminance_unit`.
-	 * @anchor		illuminanceContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ILLUMINANCE_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(illuminance, lux, luxes, lx, unit<std::ratio<1>, units::category::illuminance_unit>)
-	UNIT_ADD(illuminance, footcandle, footcandles, fc, compound_unit<luminous_flux::lumen, inverse<squared<length::foot>>>)
-	UNIT_ADD(illuminance, lumens_per_square_inch, lumens_per_square_inch, lm_per_in_sq, compound_unit<luminous_flux::lumen, inverse<squared<length::inch>>>)
-	UNIT_ADD(illuminance, phot, phots, ph, compound_unit<luminous_flux::lumens, inverse<squared<length::centimeter>>>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(illuminance)
-#endif
-
-	//------------------------------
-	//	UNITS OF RADIATION
-	//------------------------------
-
-	/**
-	 * @namespace	units::radiation
-	 * @brief		namespace for unit types and containers representing radiation values
-	 * @details		The SI units for radiation are:
-	 *				- source activity:	becquerel
-	 *				- absorbed dose:	gray
-	 *				- equivalent dose:	sievert
-	 * @anchor		radiationContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_RADIATION_UNITS)
-	UNIT_ADD_WITH_METRIC_PREFIXES(radiation, becquerel, becquerels, Bq, unit<std::ratio<1>, units::frequency::hertz>)
-	UNIT_ADD_WITH_METRIC_PREFIXES(radiation, gray, grays, Gy, compound_unit<energy::joules, inverse<mass::kilogram>>)
-	UNIT_ADD_WITH_METRIC_PREFIXES(radiation, sievert, sieverts, Sv, unit<std::ratio<1>, grays>)
-	UNIT_ADD(radiation, curie, curies, Ci, unit<std::ratio<37>, gigabecquerels>)
-	UNIT_ADD(radiation, rutherford, rutherfords, rd, unit<std::ratio<1>, megabecquerels>)
-	UNIT_ADD(radiation, rad, rads, rads, unit<std::ratio<1>, centigrays>)
-
-	UNIT_ADD_CATEGORY_TRAIT(radioactivity)
-#endif
-
-	//------------------------------
-	//	UNITS OF TORQUE
-	//------------------------------
-
-	/**
-	 * @namespace	units::torque
-	 * @brief		namespace for unit types and containers representing torque values
-	 * @details		The SI unit for torque is `newton_meters`, and the corresponding `base_unit` category is
-	 *				`torque_units`.
-	 * @anchor		torqueContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_TORQUE_UNITS)
-	UNIT_ADD(torque, newton_meter, newton_meters, Nm, unit<std::ratio<1>, units::energy::joule>)
-	UNIT_ADD(torque, foot_pound, foot_pounds, ftlb, compound_unit<length::foot, force::pounds>)
-	UNIT_ADD(torque, foot_poundal, foot_poundals, ftpdl, compound_unit<length::foot, force::poundal>)
-	UNIT_ADD(torque, inch_pound, inch_pounds, inlb, compound_unit<length::inch, force::pounds>)
-	UNIT_ADD(torque, meter_kilogram, meter_kilograms, mkgf, compound_unit<length::meter, force::kiloponds>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(torque)
-#endif
-
-	//------------------------------
-	//	AREA UNITS
-	//------------------------------
-
-	/**
-	 * @namespace	units::area
-	 * @brief		namespace for unit types and containers representing area values
-	 * @details		The SI unit for area is `square_meters`, and the corresponding `base_unit` category is
-	 *				`area_unit`.
-	 * @anchor		areaContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_AREA_UNITS)
-	UNIT_ADD(area, square_meter, square_meters, sq_m, unit<std::ratio<1>, units::category::area_unit>)
-	UNIT_ADD(area, square_foot, square_feet, sq_ft, squared<length::feet>)
-	UNIT_ADD(area, square_inch, square_inches, sq_in, squared<length::inch>)
-	UNIT_ADD(area, square_mile, square_miles, sq_mi, squared<length::miles>)
-	UNIT_ADD(area, square_kilometer, square_kilometers, sq_km, squared<length::kilometers>)
-	UNIT_ADD(area, hectare, hectares, ha, unit<std::ratio<10000>, square_meters>)
-	UNIT_ADD(area, acre, acres, acre, unit<std::ratio<43560>, square_feet>)
-	
-	UNIT_ADD_CATEGORY_TRAIT(area)
-#endif
-
-	//------------------------------
-	//	UNITS OF VOLUME
-	//------------------------------
-
-	/**
-	 * @namespace	units::volume
-	 * @brief		namespace for unit types and containers representing volume values
-	 * @details		The SI unit for volume is `cubic_meters`, and the corresponding `base_unit` category is
-	 *				`volume_unit`.
-	 * @anchor		volumeContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_VOLUME_UNITS)
-	UNIT_ADD(volume, cubic_meter, cubic_meters, cu_m, unit<std::ratio<1>, units::category::volume_unit>)
-	UNIT_ADD(volume, cubic_millimeter, cubic_millimeters, cu_mm, cubed<length::millimeter>)
-	UNIT_ADD(volume, cubic_kilometer, cubic_kilometers, cu_km, cubed<length::kilometer>)
-	UNIT_ADD_WITH_METRIC_PREFIXES(volume, liter, liters, L, cubed<deci<length::meter>>)
-	UNIT_ADD(volume, cubic_inch, cubic_inches, cu_in, cubed<length::inches>)
-	UNIT_ADD(volume, cubic_foot, cubic_feet, cu_ft, cubed<length::feet>)
-	UNIT_ADD(volume, cubic_yard, cubic_yards, cu_yd, cubed<length::yards>)
-	UNIT_ADD(volume, cubic_mile, cubic_miles, cu_mi, cubed<length::miles>)
-	UNIT_ADD(volume, gallon, gallons, gal, unit<std::ratio<231>, cubic_inches>)
-	UNIT_ADD(volume, quart, quarts, qt, unit<std::ratio<1, 4>, gallons>)
-	UNIT_ADD(volume, pint, pints, pt, unit<std::ratio<1, 2>, quarts>)
-	UNIT_ADD(volume, cup, cups, c, unit<std::ratio<1, 2>, pints>)
-	UNIT_ADD(volume, fluid_ounce, fluid_ounces, fl_oz, unit<std::ratio<1, 8>, cups>)
-	UNIT_ADD(volume, barrel, barrels, bl, unit<std::ratio<42>, gallons>)
-	UNIT_ADD(volume, bushel, bushels, bu, unit<std::ratio<215042, 100>, cubic_inches>)
-	UNIT_ADD(volume, cord, cords, cord, unit<std::ratio<128>, cubic_feet>)
-	UNIT_ADD(volume, cubic_fathom, cubic_fathoms, cu_fm, cubed<length::fathom>)
-	UNIT_ADD(volume, tablespoon, tablespoons, tbsp, unit<std::ratio<1, 2>, fluid_ounces>)
-	UNIT_ADD(volume, teaspoon, teaspoons, tsp, unit<std::ratio<1, 6>, fluid_ounces>)
-	UNIT_ADD(volume, pinch, pinches, pinch, unit<std::ratio<1, 8>, teaspoons>)
-	UNIT_ADD(volume, dash, dashes, dash, unit<std::ratio<1, 2>, pinches>)
-	UNIT_ADD(volume, drop, drops, drop, unit<std::ratio<1, 360>, fluid_ounces>)
-	UNIT_ADD(volume, fifth, fifths, fifth, unit<std::ratio<1, 5>, gallons>)
-	UNIT_ADD(volume, dram, drams, dr, unit<std::ratio<1, 8>, fluid_ounces>)
-	UNIT_ADD(volume, gill, gills, gi, unit<std::ratio<4>, fluid_ounces>)
-	UNIT_ADD(volume, peck, pecks, pk, unit<std::ratio<1, 4>, bushels>)
-	UNIT_ADD(volume, sack, sacks, sacks, unit<std::ratio<3>, bushels>)
-	UNIT_ADD(volume, shot, shots, shots, unit<std::ratio<3, 2>, fluid_ounces>)
-	UNIT_ADD(volume, strike, strikes, strikes, unit<std::ratio<2>, bushels>)
-
-	UNIT_ADD_CATEGORY_TRAIT(volume)
-#endif
-
-	//------------------------------
-	//	UNITS OF DENSITY
-	//------------------------------
-
-	/**
-	 * @namespace	units::density
-	 * @brief		namespace for unit types and containers representing density values
-	 * @details		The SI unit for density is `kilograms_per_cubic_meter`, and the corresponding `base_unit` category is
-	 *				`density_unit`.
-	 * @anchor		densityContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_DENSITY_UNITS)
-	UNIT_ADD(density, kilograms_per_cubic_meter, kilograms_per_cubic_meter, kg_per_cu_m, unit<std::ratio<1>, units::category::density_unit>)
-	UNIT_ADD(density, grams_per_milliliter, grams_per_milliliter, g_per_mL, compound_unit<mass::grams, inverse<volume::milliliter>>)
-	UNIT_ADD(density, kilograms_per_liter, kilograms_per_liter, kg_per_L, unit<std::ratio<1>, compound_unit<mass::grams, inverse<volume::milliliter>>>)
-	UNIT_ADD(density, ounces_per_cubic_foot, ounces_per_cubic_foot, oz_per_cu_ft, compound_unit<mass::ounces, inverse<volume::cubic_foot>>)
-	UNIT_ADD(density, ounces_per_cubic_inch, ounces_per_cubic_inch, oz_per_cu_in, compound_unit<mass::ounces, inverse<volume::cubic_inch>>)
-	UNIT_ADD(density, ounces_per_gallon, ounces_per_gallon, oz_per_gal, compound_unit<mass::ounces, inverse<volume::gallon>>)
-	UNIT_ADD(density, pounds_per_cubic_foot, pounds_per_cubic_foot, lb_per_cu_ft, compound_unit<mass::pounds, inverse<volume::cubic_foot>>)
-	UNIT_ADD(density, pounds_per_cubic_inch, pounds_per_cubic_inch, lb_per_cu_in, compound_unit<mass::pounds, inverse<volume::cubic_inch>>)
-	UNIT_ADD(density, pounds_per_gallon, pounds_per_gallon, lb_per_gal, compound_unit<mass::pounds, inverse<volume::gallon>>)
-	UNIT_ADD(density, slugs_per_cubic_foot, slugs_per_cubic_foot, slug_per_cu_ft, compound_unit<mass::slugs, inverse<volume::cubic_foot>>)
-
-	UNIT_ADD_CATEGORY_TRAIT(density)
-#endif
-
-	//------------------------------
-	//	UNITS OF CONCENTRATION
-	//------------------------------
-
-	/**
-	 * @namespace	units::concentration
-	 * @brief		namespace for unit types and containers representing concentration values
-	 * @details		The SI unit for concentration is `parts_per_million`, and the corresponding `base_unit` category is
-	 *				`scalar_unit`.
-	 * @anchor		concentrationContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_CONCENTRATION_UNITS)
-	UNIT_ADD(concentration, ppm, parts_per_million, ppm, unit<std::ratio<1, 1000000>, units::category::scalar_unit>)
-	UNIT_ADD(concentration, ppb, parts_per_billion, ppb, unit<std::ratio<1, 1000>, parts_per_million>)
-	UNIT_ADD(concentration, ppt, parts_per_trillion, ppt, unit<std::ratio<1, 1000>, parts_per_billion>)
-	UNIT_ADD(concentration, percent, percent, pct, unit<std::ratio<1, 100>, units::category::scalar_unit>)
-
-	UNIT_ADD_CATEGORY_TRAIT(concentration)
-#endif
-
-	//------------------------------
-	//	UNITS OF DATA
-	//------------------------------
-
-	/**
-	 * @namespace	units::data
-	 * @brief		namespace for unit types and containers representing data values
-	 * @details		The base unit for data is `bytes`, and the corresponding `base_unit` category is
-	 *				`data_unit`.
-	 * @anchor		dataContainers
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_DATA_UNITS)
-	UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(data, byte, bytes, B, unit<std::ratio<1>, units::category::data_unit>)
-	UNIT_ADD(data, exabyte, exabytes, EB, unit<std::ratio<1000>, petabytes>)
-	UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(data, bit, bits, b, unit<std::ratio<1, 8>, byte>)
-	UNIT_ADD(data, exabit, exabits, Eb, unit<std::ratio<1000>, petabits>)
-
-	UNIT_ADD_CATEGORY_TRAIT(data)
-#endif
-
-	//------------------------------
-	//	UNITS OF DATA TRANSFER
-	//------------------------------
-
-	/**
-	* @namespace	units::data_transfer_rate
-	* @brief		namespace for unit types and containers representing data values
-	* @details		The base unit for data is `bytes`, and the corresponding `base_unit` category is
-	*				`data_unit`.
-	* @anchor		dataContainers
-	* @sa			See unit_t for more information on unit type containers.
-	*/
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_DATA_TRANSFER_RATE_UNITS)
-	UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(data_transfer_rate, bytes_per_second, bytes_per_second, Bps, unit<std::ratio<1>, units::category::data_transfer_rate_unit>)
-	UNIT_ADD(data_transfer_rate, exabytes_per_second, exabytes_per_second, EBps, unit<std::ratio<1000>, petabytes_per_second>)
-	UNIT_ADD_WITH_METRIC_AND_BINARY_PREFIXES(data_transfer_rate, bits_per_second, bits_per_second, bps, unit<std::ratio<1, 8>, bytes_per_second>)
-	UNIT_ADD(data_transfer_rate, exabits_per_second, exabits_per_second, Ebps, unit<std::ratio<1000>, petabits_per_second>)
-
-	UNIT_ADD_CATEGORY_TRAIT(data_transfer_rate)
-#endif
-
-	//------------------------------
-	//	CONSTANTS
-	//------------------------------
-
-	/**
-	 * @brief		namespace for physical constants like PI and Avogadro's Number.
-	 * @sa			See unit_t for more information on unit type containers.
-	 */
-#if !defined(DISABLE_PREDEFINED_UNITS)
-	namespace constants
-	{
-		/**
-		 * @name Unit Containers
-		 * @anchor constantContainers
-		 * @{
-		 */
-		using PI = unit<std::ratio<1>, dimensionless::scalar, std::ratio<1>>;
-
-		static constexpr const unit_t<PI>																											pi(1);											///< Ratio of a circle's circumference to its diameter.
-		static constexpr const velocity::meters_per_second_t																						c(299792458.0);									///< Speed of light in vacuum.
-		static constexpr const unit_t<compound_unit<cubed<length::meters>, inverse<mass::kilogram>, inverse<squared<time::seconds>>>>				G(6.67408e-11);									///< Newtonian constant of gravitation.
-		static constexpr const unit_t<compound_unit<energy::joule, time::seconds>>																	h(6.626070040e-34);								///< Planck constant.
-		static constexpr const unit_t<compound_unit<force::newtons, inverse<squared<current::ampere>>>>												mu0(pi * 4.0e-7 * force::newton_t(1) / units::math::cpow<2>(current::ampere_t(1)));										///< vacuum permeability.
-		static constexpr const unit_t<compound_unit<capacitance::farad, inverse<length::meter>>>													epsilon0(1.0 / (mu0 * math::cpow<2>(c)));		///< vacuum permitivity.
-		static constexpr const impedance::ohm_t																										Z0(mu0 * c);									///< characteristic impedance of vacuum.
-		static constexpr const unit_t<compound_unit<force::newtons, area::square_meter, inverse<squared<charge::coulomb>>>>							k_e(1.0 / (4 * pi * epsilon0));					///< Coulomb's constant.
-		static constexpr const charge::coulomb_t																									e(1.6021766208e-19);							///< elementary charge.
-		static constexpr const mass::kilogram_t																										m_e(9.10938356e-31);							///< electron mass.
-		static constexpr const mass::kilogram_t																										m_p(1.672621898e-27);							///< proton mass.
-		static constexpr const unit_t<compound_unit<energy::joules, inverse<magnetic_field_strength::tesla>>>										mu_B(e * h / (4 * pi *m_e));					///< Bohr magneton.
-		static constexpr const unit_t<inverse<substance::mol>>																						N_A(6.022140857e23);							///< Avagadro's Number.
-		static constexpr const unit_t<compound_unit<energy::joules, inverse<temperature::kelvin>, inverse<substance::moles>>>						R(8.3144598);									///< Gas constant.
-		static constexpr const unit_t<compound_unit<energy::joules, inverse<temperature::kelvin>>>													k_B(R / N_A);									///< Boltzmann constant.
-		static constexpr const unit_t<compound_unit<charge::coulomb, inverse<substance::mol>>>														F(N_A * e);										///< Faraday constant.
-		static constexpr const unit_t<compound_unit<power::watts, inverse<area::square_meters>, inverse<squared<squared<temperature::kelvin>>>>>	sigma((2 * math::cpow<5>(pi) * math::cpow<4>(R)) / (15 * math::cpow<3>(h) * math::cpow<2>(c) * math::cpow<4>(N_A)));	///< Stefan-Boltzmann constant.
-		/** @} */
-	}
-#endif
-
-	//----------------------------------
-	//	UNIT-ENABLED CMATH FUNCTIONS
-	//----------------------------------
-
-	/**
-	 * @brief		namespace for unit-enabled versions of the `<cmath>` library
-	 * @details		Includes trigonometric functions, exponential/log functions, rounding functions, etc.
-	 * @sa			See `unit_t` for more information on unit type containers.
-	 */
-	namespace math
-	{
-
-		//----------------------------------
-		//	MIN/MAX FUNCTIONS
-		//----------------------------------
-
-		template<class UnitTypeLhs, class UnitTypeRhs>
-		UnitTypeLhs min(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
-		{
-			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Unit types are not compatible.");
-			UnitTypeLhs r(rhs);
-			return (lhs < r ? lhs : r);
-		}
-
-		template<class UnitTypeLhs, class UnitTypeRhs>
-		UnitTypeLhs max(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs)
-		{
-			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Unit types are not compatible.");
-			UnitTypeLhs r(rhs);
-			return (lhs > r ? lhs : r);
-		}
-
-		//----------------------------------
-		//	TRIGONOMETRIC FUNCTIONS
-		//----------------------------------
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute cosine
-		 * @details		The input value can be in any unit of angle, including radians or degrees.
-		 * @tparam		AngleUnit	any `unit_t` type of `category::angle_unit`. 
-		 * @param[in]	angle		angle to compute the cosine of
-		 * @returns		Returns the cosine of <i>angle</i>
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class AngleUnit>
-		dimensionless::scalar_t cos(const AngleUnit angle) noexcept
-		{
-			static_assert(traits::is_angle_unit<AngleUnit>::value, "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
-			return dimensionless::scalar_t(std::cos(angle.template convert<angle::radian>()()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute sine
-		 * @details		The input value can be in any unit of angle, including radians or degrees.
-		 * @tparam		AngleUnit	any `unit_t` type of `category::angle_unit`.
-		 * @param[in]	angle		angle to compute the since of
-		 * @returns		Returns the sine of <i>angle</i>
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class AngleUnit>
-		dimensionless::scalar_t sin(const AngleUnit angle) noexcept
-		{
-			static_assert(traits::is_angle_unit<AngleUnit>::value, "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
-			return dimensionless::scalar_t(std::sin(angle.template convert<angle::radian>()()));
-		}
-#endif
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute tangent
-		 * @details		The input value can be in any unit of angle, including radians or degrees.
-		 * @tparam		AngleUnit	any `unit_t` type of `category::angle_unit`.
-		 * @param[in]	angle		angle to compute the tangent of
-		 * @returns		Returns the tangent of <i>angle</i>
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class AngleUnit>
-		dimensionless::scalar_t tan(const AngleUnit angle) noexcept
-		{
-			static_assert(traits::is_angle_unit<AngleUnit>::value, "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
-			return dimensionless::scalar_t(std::tan(angle.template convert<angle::radian>()()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute arc cosine
-		 * @details		Returns the principal value of the arc cosine of x, expressed in radians.
-		 * @param[in]	x		Value whose arc cosine is computed, in the interval [-1,+1].
-		 * @returns		Principal arc cosine of x, in the interval [0,pi] radians.
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class ScalarUnit>
-		angle::radian_t acos(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return angle::radian_t(std::acos(x()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute arc sine
-		 * @details		Returns the principal value of the arc sine of x, expressed in radians.
-		 * @param[in]	x		Value whose arc sine is computed, in the interval [-1,+1].
-		 * @returns		Principal arc sine of x, in the interval [-pi/2,+pi/2] radians.
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class ScalarUnit>
-		angle::radian_t asin(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return angle::radian_t(std::asin(x()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute arc tangent
-		 * @details		Returns the principal value of the arc tangent of x, expressed in radians. 
-		 *				Notice that because of the sign ambiguity, the function cannot determine with 
-		 *				certainty in which quadrant the angle falls only by its tangent value. See 
-		 *				atan2 for an alternative that takes a fractional argument instead.
-		 * @tparam		AngleUnit	any `unit_t` type of `category::angle_unit`.
-		 * @param[in]	x		Value whose arc tangent is computed, in the interval [-1,+1].
-		 * @returns		Principal arc tangent of x, in the interval [-pi/2,+pi/2] radians.
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class ScalarUnit>
-		angle::radian_t atan(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return angle::radian_t(std::atan(x()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute arc tangent with two parameters
-		 * @details		To compute the value, the function takes into account the sign of both arguments in order to determine the quadrant.
-		 * @param[in]	y		y-component of the triangle expressed.
-		 * @param[in]	x		x-component of the triangle expressed.
-		 * @returns		Returns the principal value of the arc tangent of <i>y/x</i>, expressed in radians.
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class Y, class X>
-		angle::radian_t atan2(const Y y, const X x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<decltype(y/x)>::value, "The quantity y/x must yield a dimensionless ratio.");
-
-			// X and Y could be different length units, so normalize them
-			return angle::radian_t(std::atan2(y.template convert<typename units::traits::unit_t_traits<X>::unit_type>()(), x()));
-		}
-#endif
-
-		//----------------------------------
-		//	HYPERBOLIC TRIG FUNCTIONS
-		//----------------------------------
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute hyperbolic cosine
-		 * @details		The input value can be in any unit of angle, including radians or degrees.
-		 * @tparam		AngleUnit	any `unit_t` type of `category::angle_unit`.
-		 * @param[in]	angle		angle to compute the hyperbolic cosine of
-		 * @returns		Returns the hyperbolic cosine of <i>angle</i>
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class AngleUnit>
-		dimensionless::scalar_t cosh(const AngleUnit angle) noexcept
-		{
-			static_assert(traits::is_angle_unit<AngleUnit>::value, "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
-			return dimensionless::scalar_t(std::cosh(angle.template convert<angle::radian>()()));
-		}
-#endif
-
-		/**
-		* @ingroup		UnitMath
-		* @brief		Compute hyperbolic sine
-		* @details		The input value can be in any unit of angle, including radians or degrees.
-		* @tparam		AngleUnit	any `unit_t` type of `category::angle_unit`.
-		* @param[in]	angle		angle to compute the hyperbolic sine of
-		* @returns		Returns the hyperbolic sine of <i>angle</i>
-		*/
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class AngleUnit>
-		dimensionless::scalar_t sinh(const AngleUnit angle) noexcept
-		{
-			static_assert(traits::is_angle_unit<AngleUnit>::value, "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
-			return dimensionless::scalar_t(std::sinh(angle.template convert<angle::radian>()()));
-		}
-#endif
-
-		/**
-		* @ingroup		UnitMath
-		* @brief		Compute hyperbolic tangent
-		* @details		The input value can be in any unit of angle, including radians or degrees.
-		* @tparam		AngleUnit	any `unit_t` type of `category::angle_unit`.
-		* @param[in]	angle		angle to compute the hyperbolic tangent of
-		* @returns		Returns the hyperbolic tangent of <i>angle</i>
-		*/
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class AngleUnit>
-		dimensionless::scalar_t tanh(const AngleUnit angle) noexcept
-		{
-			static_assert(traits::is_angle_unit<AngleUnit>::value, "Type `AngleUnit` must be a unit of angle derived from `unit_t`.");
-			return dimensionless::scalar_t(std::tanh(angle.template convert<angle::radian>()()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute arc hyperbolic cosine
-		 * @details		Returns the nonnegative arc hyperbolic cosine of x, expressed in radians.
-		 * @param[in]	x	Value whose arc hyperbolic cosine is computed. If the argument is less
-		 *					than 1, a domain error occurs.
-		 * @returns		Nonnegative arc hyperbolic cosine of x, in the interval [0,+INFINITY] radians.
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class ScalarUnit>
-		angle::radian_t acosh(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return angle::radian_t(std::acosh(x()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute arc hyperbolic sine
-		 * @details		Returns the arc hyperbolic sine of x, expressed in radians.
-		 * @param[in]	x	Value whose arc hyperbolic sine is computed.
-		 * @returns		Arc hyperbolic sine of x, in radians.
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class ScalarUnit>
-		angle::radian_t asinh(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return angle::radian_t(std::asinh(x()));
-		}
-#endif
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute arc hyperbolic tangent
-		 * @details		Returns the arc hyperbolic tangent of x, expressed in radians.
-		 * @param[in]	x	Value whose arc hyperbolic tangent is computed, in the interval [-1,+1]. 
-		 *					If the argument is out of this interval, a domain error occurs. For 
-		 *					values of -1 and +1, a pole error may occur.
-		 * @returns		units::angle::radian_t
-		 */
-#if !defined(DISABLE_PREDEFINED_UNITS) || defined(ENABLE_PREDEFINED_ANGLE_UNITS)
-		template<class ScalarUnit>
-		angle::radian_t atanh(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return angle::radian_t(std::atanh(x()));
-		}
-#endif
-
-		//----------------------------------
-		//	TRANSCENDENTAL FUNCTIONS
-		//----------------------------------
-
-		// it makes NO SENSE to put dimensioned units into a transcendental function, and if you think it does you are
-		// demonstrably wrong. https://en.wikipedia.org/wiki/Transcendental_function#Dimensional_analysis
-		
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute exponential function
-		 * @details		Returns the base-e exponential function of x, which is e raised to the power x: ex.
-		 * @param[in]	x	scalar value of the exponent.
-		 * @returns		Exponential value of x.
-		 *				If the magnitude of the result is too large to be represented by a value of the return type, the
-		 *				function returns HUGE_VAL (or HUGE_VALF or HUGE_VALL) with the proper sign, and an overflow range error occurs
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t exp(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return dimensionless::scalar_t(std::exp(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute natural logarithm
-		 * @details		Returns the natural logarithm of x.
-		 * @param[in]	x	scalar value whose logarithm is calculated. If the argument is negative, a 
-		 *					domain error occurs.
-		 * @sa			log10 for more common base-10 logarithms
-		 * @returns		Natural logarithm of x.
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t log(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return dimensionless::scalar_t(std::log(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute common logarithm
-		 * @details		Returns the common (base-10) logarithm of x.
-		 * @param[in]	x	Value whose logarithm is calculated. If the argument is negative, a 
-		 *					domain error occurs.
-		 * @returns		Common logarithm of x.
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t log10(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return dimensionless::scalar_t(std::log10(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Break into fractional and integral parts.
-		 * @details		The integer part is stored in the object pointed by intpart, and the 
-		 *				fractional part is returned by the function. Both parts have the same sign 
-		 *				as x.
-		 * @param[in]	x		scalar value to break into parts.
-		 * @param[in]	intpart Pointer to an object (of the same type as x) where the integral part
-		 *				is stored with the same sign as x.
-		 * @returns		The fractional part of x, with the same sign.
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t modf(const ScalarUnit x, ScalarUnit* intpart) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-
-			UNIT_LIB_DEFAULT_TYPE intp;
-			dimensionless::scalar_t fracpart = dimensionless::scalar_t(std::modf(x(), &intp));
-			*intpart = intp;
-			return fracpart;
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute binary exponential function
-		 * @details		Returns the base-2 exponential function of x, which is 2 raised to the power x: 2^x.
-		 * 2param[in]	x	Value of the exponent.
-		 * @returns		2 raised to the power of x.
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t exp2(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return dimensionless::scalar_t(std::exp2(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute exponential minus one
-		 * @details		Returns e raised to the power x minus one: e^x-1. For small magnitude values 
-		 *				of x, expm1 may be more accurate than exp(x)-1.
-		 * @param[in]	x	Value of the exponent.
-		 * @returns		e raised to the power of x, minus one.
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t expm1(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return dimensionless::scalar_t(std::expm1(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute logarithm plus one
-		 * @details		Returns the natural logarithm of one plus x. For small magnitude values of 
-		 *				x, logp1 may be more accurate than log(1+x).
-		 * @param[in]	x	Value whose logarithm is calculated. If the argument is less than -1, a 
-		 *					domain error occurs.
-		 * @returns		The natural logarithm of (1+x).
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t log1p(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return dimensionless::scalar_t(std::log1p(x()));
-		}
-		
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute binary logarithm
-		 * @details		Returns the binary (base-2) logarithm of x.
-		 * @param[in]	x	Value whose logarithm is calculated. If the argument is negative, a 
-		 *					domain error occurs.
-		 * @returns		The binary logarithm of x: log2x.
-		 */
-		template<class ScalarUnit>
-		dimensionless::scalar_t log2(const ScalarUnit x) noexcept
-		{
-			static_assert(traits::is_dimensionless_unit<ScalarUnit>::value, "Type `ScalarUnit` must be a dimensionless unit derived from `unit_t`.");
-			return dimensionless::scalar_t(std::log2(x()));
-		}
-
-		//----------------------------------
-		//	POWER FUNCTIONS
-		//----------------------------------
-		
-		/* pow is implemented earlier in the library since a lot of the unit definitions depend on it */
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		computes the square root of <i>value</i>
-		 * @details		Only implemented for linear_scale units.
-		 * @param[in]	value `unit_t` derived type to compute the square root of.
-		 * @returns		new unit_t, whose units are the square root of value's. E.g. if values
-		 *				had units of `square_meter`, then the return type will have units of
-		 *				`meter`.
-		 * @note		`sqrt` provides a _rational approximation_ of the square root of <i>value</i>.
-		 *				In some cases, _both_ the returned value _and_ conversion factor of the returned
-		 *				unit type may have errors no larger than `1e-10`.
-		 */
-		template<class UnitType, std::enable_if_t<units::traits::has_linear_scale<UnitType>::value, int> = 0>
-		inline auto sqrt(const UnitType& value) noexcept -> unit_t<square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
-		{
-			return unit_t<square_root<typename units::traits::unit_t_traits<UnitType>::unit_type>, typename units::traits::unit_t_traits<UnitType>::underlying_type, linear_scale>
-				(std::sqrt(value()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Computes the square root of the sum-of-squares of x and y.
-		 * @details		Only implemented for linear_scale units.
-		 * @param[in]	x	unit_t type value
-		 * @param[in]	y	unit_t type value
-		 * @returns		square root of the sum-of-squares of x and y in the same units
-		 *				as x.
-		 */
-		template<class UnitTypeLhs, class UnitTypeRhs, std::enable_if_t<units::traits::has_linear_scale<UnitTypeLhs, UnitTypeRhs>::value, int> = 0>
-		inline UnitTypeLhs hypot(const UnitTypeLhs& x, const UnitTypeRhs& y)
-		{
-			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Parameters of hypot() function are not compatible units.");
-			return UnitTypeLhs(std::hypot(x(), y.template convert<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
-		}
-
-		//----------------------------------
-		//	ROUNDING FUNCTIONS
-		//----------------------------------
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Round up value
-		 * @details		Rounds x upward, returning the smallest integral value that is not less than x.
-		 * @param[in]	x	Unit value to round up.
-		 * @returns		The smallest integral value that is not less than x.
-		 */
-		template<class UnitType, class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
-		UnitType ceil(const UnitType x) noexcept
-		{
-			return UnitType(std::ceil(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Round down value
-		 * @details		Rounds x downward, returning the largest integral value that is not greater than x.
-		 * @param[in]	x	Unit value to round down.
-		 * @returns		The value of x rounded downward.
-		 */
-		template<class UnitType, class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
-		UnitType floor(const UnitType x) noexcept
-		{
-			return UnitType(std::floor(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute remainder of division
-		 * @details		Returns the floating-point remainder of numer/denom (rounded towards zero).
-		 * @param[in]	numer	Value of the quotient numerator.
-		 * @param[in]	denom	Value of the quotient denominator.
-		 * @returns		The remainder of dividing the arguments.
-		 */
-		template<class UnitTypeLhs, class UnitTypeRhs, class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value && traits::is_unit_t<UnitTypeRhs>::value>>
-		UnitTypeLhs fmod(const UnitTypeLhs numer, const UnitTypeRhs denom) noexcept
-		{
-			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Parameters of fmod() function are not compatible units.");
-			return UnitTypeLhs(std::fmod(numer(), denom.template convert<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Truncate value
-		 * @details		Rounds x toward zero, returning the nearest integral value that is not 
-		 *				larger in magnitude than x. Effectively rounds towards 0.
-		 * @param[in]	x	Value to truncate
-		 * @returns		The nearest integral value that is not larger in magnitude than x.
-		 */
-		template<class UnitType, class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
-		UnitType trunc(const UnitType x) noexcept
-		{
-			return UnitType(std::trunc(x()));
-		}
-
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Round to nearest
-		 * @details		Returns the integral value that is nearest to x, with halfway cases rounded
-		 *				away from zero.
-		 * @param[in]	x	value to round.
-		 * @returns		The value of x rounded to the nearest integral.
-		 */
-		template<class UnitType, class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
-		UnitType round(const UnitType x) noexcept
-		{
-			return UnitType(std::round(x()));
-		}
-
-		//----------------------------------
-		//	FLOATING POINT MANIPULATION 
-		//----------------------------------
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Copy sign
-		 * @details		Returns a value with the magnitude and dimension of x, and the sign of y. 
-		 *				Values x and y do not have to be compatible units.
-		 * @param[in]	x	Value with the magnitude of the resulting value.
-		 * @param[in]	y	Value with the sign of the resulting value.
-		 * @returns		value with the magnitude and dimension of x, and the sign of y.
-		 */
-		template<class UnitTypeLhs, class UnitTypeRhs, class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value && traits::is_unit_t<UnitTypeRhs>::value>>
-		UnitTypeLhs copysign(const UnitTypeLhs x, const UnitTypeRhs y) noexcept
-		{
-			return UnitTypeLhs(std::copysign(x(), y()));	// no need for conversion to get the correct sign.
-		}
-
-		/// Overload to copy the sign from a raw double
-		template<class UnitTypeLhs, class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value>>
-		UnitTypeLhs copysign(const UnitTypeLhs x, const UNIT_LIB_DEFAULT_TYPE y) noexcept
-		{
-			return UnitTypeLhs(std::copysign(x(), y));
-		}
-
-		//----------------------------------
-		//	MIN / MAX / DIFFERENCE 
-		//----------------------------------
-		
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Positive difference
-		 * @details		The function returns x-y if x>y, and zero otherwise, in the same units as x.
-		 *				Values x and y do not have to be the same type of units, but they do have to
-		 *				be compatible.
-		 * @param[in]	x	Values whose difference is calculated.
-		 * @param[in]	y	Values whose difference is calculated.
-		 * @returns		The positive difference between x and y.
-		 */
-		template<class UnitTypeLhs, class UnitTypeRhs, class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value && traits::is_unit_t<UnitTypeRhs>::value>>
-		UnitTypeLhs fdim(const UnitTypeLhs x, const UnitTypeRhs y) noexcept
-		{
-			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Parameters of fdim() function are not compatible units.");
-			return UnitTypeLhs(std::fdim(x(), y.template convert<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Maximum value
-		 * @details		Returns the larger of its arguments: either x or y, in the same units as x.
-		 *				Values x and y do not have to be the same type of units, but they do have to
-		 *				be compatible.
-		 * @param[in]	x	Values among which the function selects a maximum.
-		 * @param[in]	y	Values among which the function selects a maximum.
-		 * @returns		The maximum numeric value of its arguments.
-		 */
-		template<class UnitTypeLhs, class UnitTypeRhs, class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value && traits::is_unit_t<UnitTypeRhs>::value>>
-		UnitTypeLhs fmax(const UnitTypeLhs x, const UnitTypeRhs y) noexcept
-		{
-			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Parameters of fmax() function are not compatible units.");
-			return UnitTypeLhs(std::fmax(x(), y.template convert<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Minimum value
-		 * @details		Returns the smaller of its arguments: either x or y, in the same units as x.
-		 *				If one of the arguments in a NaN, the other is returned.
-		 *				Values x and y do not have to be the same type of units, but they do have to
-		 *				be compatible.
-		 * @param[in]	x	Values among which the function selects a minimum.
-		 * @param[in]	y	Values among which the function selects a minimum.
-		 * @returns		The minimum numeric value of its arguments.
-		 */
-		template<class UnitTypeLhs, class UnitTypeRhs, class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value && traits::is_unit_t<UnitTypeRhs>::value>>
-		UnitTypeLhs fmin(const UnitTypeLhs x, const UnitTypeRhs y) noexcept
-		{
-			static_assert(traits::is_convertible_unit_t<UnitTypeLhs, UnitTypeRhs>::value, "Parameters of fmin() function are not compatible units.");
-			return UnitTypeLhs(std::fmin(x(), y.template convert<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type>()()));
-		}
-
-		//----------------------------------
-		//	OTHER FUNCTIONS
-		//----------------------------------
-		
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute absolute value
-		 * @details		Returns the absolute value of x, i.e. |x|.
-		 * @param[in]	x	Value whose absolute value is returned.
-		 * @returns		The absolute value of x.
-		 */
-		template<class UnitType, class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
-		UnitType fabs(const UnitType x) noexcept
-		{
-			return UnitType(std::fabs(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Compute absolute value
-		 * @details		Returns the absolute value of x, i.e. |x|.
-		 * @param[in]	x	Value whose absolute value is returned.
-		 * @returns		The absolute value of x.
-		 */
-		template<class UnitType, class = std::enable_if_t<traits::is_unit_t<UnitType>::value>>
-		UnitType abs(const UnitType x) noexcept
-		{
-			return UnitType(std::fabs(x()));
-		}
-
-		/**
-		 * @ingroup		UnitMath
-		 * @brief		Multiply-add
-		 * @details		Returns x*y+z. The function computes the result without losing precision in 
-		 *				any intermediate result. The resulting unit type is a compound unit of x* y.
-		 * @param[in]	x	Values to be multiplied.
-		 * @param[in]	y	Values to be multiplied.
-		 * @param[in]	z	Value to be added.
-		 * @returns		The result of x*y+z
-		 */
-		template<class UnitTypeLhs, class UnitMultiply, class UnitAdd, class = std::enable_if_t<traits::is_unit_t<UnitTypeLhs>::value && traits::is_unit_t<UnitMultiply>::value && traits::is_unit_t<UnitAdd>::value>>
-		auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept -> decltype(x * y)
-		{
-			using resultType = decltype(x * y);
-			static_assert(traits::is_convertible_unit_t<compound_unit<typename units::traits::unit_t_traits<UnitTypeLhs>::unit_type, typename units::traits::unit_t_traits<UnitMultiply>::unit_type>, typename units::traits::unit_t_traits<UnitAdd>::unit_type>::value, "Unit types are not compatible.");
-			return resultType(std::fma(x(), y(), resultType(z)()));
-		}
-
-	}	// end namespace math
-}	// end namespace units
-
-//------------------------------
-//	std::numeric_limits
-//------------------------------
-
-namespace std
-{
-	template<class Units, typename T, template<typename> class NonLinearScale>
-	class numeric_limits<units::unit_t<Units, T, NonLinearScale>>
-	{
-	public:
-		static constexpr units::unit_t<Units, T, NonLinearScale> min()
-		{
-			return units::unit_t<Units, T, NonLinearScale>(std::numeric_limits<T>::min());
-		}
-		static constexpr units::unit_t<Units, T, NonLinearScale> max()
-		{
-			return units::unit_t<Units, T, NonLinearScale>(std::numeric_limits<T>::max());
-		}
-		static constexpr units::unit_t<Units, T, NonLinearScale> lowest()
-		{
-			return units::unit_t<Units, T, NonLinearScale>(std::numeric_limits<T>::lowest());
-		}
-	};
-}
-
-#ifdef _MSC_VER
-#	if _MSC_VER <= 1800
-#		pragma warning(pop)
-#		undef constexpr
-#		pragma pop_macro("constexpr")
-#		undef noexcept
-#		pragma pop_macro("noexcept")
-#		undef _ALLOW_KEYWORD_MACROS
-#	endif // _MSC_VER < 1800
-#	pragma pop_macro("pascal")
-#endif // _MSC_VER
-
-#endif // units_h__
-
-// For Emacs
-// Local Variables:
-// Mode: C++
-// c-basic-offset: 2
-// fill-column: 116
-// tab-width: 4
-// End:
-
-using namespace units::literals;
-
-namespace units {
-using namespace acceleration;
-using namespace angular_velocity;
-using namespace length;
-using namespace time;
-using namespace velocity;
-using namespace acceleration;
-using namespace angle;
-using namespace voltage;
-}  // namespace units

wpiutil/src/main/native/include/wpi/Chrono.h

diff --git a/wpiutil/src/main/native/include/wpi/Chrono.h b/wpiutil/src/main/native/include/wpi/Chrono.h
index 4593e85..33341c6 100644
--- a/wpiutil/src/main/native/include/wpi/Chrono.h
+++ b/wpiutil/src/main/native/include/wpi/Chrono.h
@@ -22,7 +22,7 @@
 namespace sys {
 
 /// A time point on the system clock. This is provided for two reasons:
-/// - to insulate us agains subtle differences in behavoir to differences in
+/// - to insulate us against subtle differences in behavior to differences in
 ///   system clock precision (which is implementation-defined and differs between
 ///   platforms).
 /// - to shorten the type name

wpiutil/src/main/native/include/wpi/Demangle.h

diff --git a/wpiutil/src/main/native/include/wpi/Demangle.h b/wpiutil/src/main/native/include/wpi/Demangle.h
index b0e1134..5989ccc 100644
--- a/wpiutil/src/main/native/include/wpi/Demangle.h
+++ b/wpiutil/src/main/native/include/wpi/Demangle.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,6 +10,8 @@
 
 #include <string>
 
+#include "wpi/Twine.h"
+
 namespace wpi {
 
 /**
@@ -18,7 +20,7 @@
  * @param mangledSymbol the mangled symbol.
  * @return The demangled symbol, or mangledSymbol if demangling fails.
  */
-std::string Demangle(char const* mangledSymbol);
+std::string Demangle(const Twine& mangledSymbol);
 
 }  // namespace wpi
 

wpiutil/src/main/native/include/wpi/Endian.h

diff --git a/wpiutil/src/main/native/include/wpi/Endian.h b/wpiutil/src/main/native/include/wpi/Endian.h
index 365209c..ee4a51d 100644
--- a/wpiutil/src/main/native/include/wpi/Endian.h
+++ b/wpiutil/src/main/native/include/wpi/Endian.h
@@ -68,7 +68,9 @@
 /// Swap the bytes of value to match the given endianness.
 template<typename value_type, endianness endian>
 inline value_type byte_swap(value_type value) {
-  return byte_swap(value, endian);
+  if constexpr ((endian != native) && (endian != system_endianness()))
+    sys::swapByteOrder(value);
+  return value;
 }
 
 /// Read a value of a particular endianness from memory.
@@ -87,7 +89,13 @@
          endianness endian,
          std::size_t alignment>
 inline value_type read(const void *memory) {
-  return read<value_type, alignment>(memory, endian);
+  value_type ret;
+
+  memcpy(&ret,
+         LLVM_ASSUME_ALIGNED(
+             memory, (detail::PickAlignment<value_type, alignment>::value)),
+         sizeof(value_type));
+  return byte_swap<value_type, endian>(ret);
 }
 
 /// Read a value of a particular endianness from a buffer, and increment the
@@ -102,7 +110,9 @@
 template<typename value_type, endianness endian, std::size_t alignment,
          typename CharT>
 inline value_type readNext(const CharT *&memory) {
-  return readNext<value_type, alignment, CharT>(memory, endian);
+  value_type ret = read<value_type, endian, alignment>(memory);
+  memory += sizeof(value_type);
+  return ret;
 }
 
 /// Write a value to memory with a particular endianness.
@@ -118,12 +128,12 @@
          endianness endian,
          std::size_t alignment>
 inline void write(void *memory, value_type value) {
-  write<value_type, alignment>(memory, value, endian);
+  value = byte_swap<value_type, endian>(value);
+  memcpy(LLVM_ASSUME_ALIGNED(
+             memory, (detail::PickAlignment<value_type, alignment>::value)),
+         &value, sizeof(value_type));
 }
 
-template <typename value_type>
-using make_unsigned_t = typename std::make_unsigned<value_type>::type;
-
 /// Read a value of a particular endianness from memory, for a location
 /// that starts at the given bit offset within the first byte.
 template <typename value_type, endianness endian, std::size_t alignment>
@@ -142,15 +152,15 @@
     val[1] = byte_swap<value_type, endian>(val[1]);
 
     // Shift bits from the lower value into place.
-    make_unsigned_t<value_type> lowerVal = val[0] >> startBit;
+    std::make_unsigned_t<value_type> lowerVal = val[0] >> startBit;
     // Mask off upper bits after right shift in case of signed type.
-    make_unsigned_t<value_type> numBitsFirstVal =
+    std::make_unsigned_t<value_type> numBitsFirstVal =
         (sizeof(value_type) * 8) - startBit;
-    lowerVal &= ((make_unsigned_t<value_type>)1 << numBitsFirstVal) - 1;
+    lowerVal &= ((std::make_unsigned_t<value_type>)1 << numBitsFirstVal) - 1;
 
     // Get the bits from the upper value.
-    make_unsigned_t<value_type> upperVal =
-        val[1] & (((make_unsigned_t<value_type>)1 << startBit) - 1);
+    std::make_unsigned_t<value_type> upperVal =
+        val[1] & (((std::make_unsigned_t<value_type>)1 << startBit) - 1);
     // Shift them in to place.
     upperVal <<= numBitsFirstVal;
 
@@ -178,15 +188,15 @@
 
     // Mask off any existing bits in the upper part of the lower value that
     // we want to replace.
-    val[0] &= ((make_unsigned_t<value_type>)1 << startBit) - 1;
-    make_unsigned_t<value_type> numBitsFirstVal =
+    val[0] &= ((std::make_unsigned_t<value_type>)1 << startBit) - 1;
+    std::make_unsigned_t<value_type> numBitsFirstVal =
         (sizeof(value_type) * 8) - startBit;
-    make_unsigned_t<value_type> lowerVal = value;
+    std::make_unsigned_t<value_type> lowerVal = value;
     if (startBit > 0) {
       // Mask off the upper bits in the new value that are not going to go into
       // the lower value. This avoids a left shift of a negative value, which
       // is undefined behavior.
-      lowerVal &= (((make_unsigned_t<value_type>)1 << numBitsFirstVal) - 1);
+      lowerVal &= (((std::make_unsigned_t<value_type>)1 << numBitsFirstVal) - 1);
       // Now shift the new bits into place
       lowerVal <<= startBit;
     }
@@ -194,11 +204,11 @@
 
     // Mask off any existing bits in the lower part of the upper value that
     // we want to replace.
-    val[1] &= ~(((make_unsigned_t<value_type>)1 << startBit) - 1);
+    val[1] &= ~(((std::make_unsigned_t<value_type>)1 << startBit) - 1);
     // Next shift the bits that go into the upper value into position.
-    make_unsigned_t<value_type> upperVal = value >> numBitsFirstVal;
+    std::make_unsigned_t<value_type> upperVal = value >> numBitsFirstVal;
     // Mask off upper bits after right shift in case of signed type.
-    upperVal &= ((make_unsigned_t<value_type>)1 << startBit) - 1;
+    upperVal &= ((std::make_unsigned_t<value_type>)1 << startBit) - 1;
     val[1] |= upperVal;
 
     // Finally, rewrite values.

wpiutil/src/main/native/include/wpi/ErrorHandling.h

diff --git a/wpiutil/src/main/native/include/wpi/ErrorHandling.h b/wpiutil/src/main/native/include/wpi/ErrorHandling.h
index ef5843b..5c89c5e 100644
--- a/wpiutil/src/main/native/include/wpi/ErrorHandling.h
+++ b/wpiutil/src/main/native/include/wpi/ErrorHandling.h
@@ -46,7 +46,7 @@
   void install_fatal_error_handler(fatal_error_handler_t handler,
                                    void *user_data = nullptr);
 
-  /// Restores default error handling behaviour.
+  /// Restores default error handling behavior.
   void remove_fatal_error_handler();
 
   /// ScopedFatalErrorHandler - This is a simple helper class which just

wpiutil/src/main/native/include/wpi/HttpServerConnection.h

diff --git a/wpiutil/src/main/native/include/wpi/HttpServerConnection.h b/wpiutil/src/main/native/include/wpi/HttpServerConnection.h
index c49c2a2..17c9ca2 100644
--- a/wpiutil/src/main/native/include/wpi/HttpServerConnection.h
+++ b/wpiutil/src/main/native/include/wpi/HttpServerConnection.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -67,9 +67,9 @@
    *                      be set to false.
    * @param extra Extra HTTP headers to send, including final "\r\n"
    */
-  void BuildHeader(raw_ostream& os, int code, const Twine& codeText,
-                   const Twine& contentType, uint64_t contentLength,
-                   const Twine& extra = Twine{});
+  virtual void BuildHeader(raw_ostream& os, int code, const Twine& codeText,
+                           const Twine& contentType, uint64_t contentLength,
+                           const Twine& extra = Twine{});
 
   /**
    * Send data to client.
@@ -94,8 +94,9 @@
    * @param content Response message content
    * @param extraHeader Extra HTTP headers to send, including final "\r\n"
    */
-  void SendResponse(int code, const Twine& codeText, const Twine& contentType,
-                    StringRef content, const Twine& extraHeader = Twine{});
+  virtual void SendResponse(int code, const Twine& codeText,
+                            const Twine& contentType, StringRef content,
+                            const Twine& extraHeader = Twine{});
 
   /**
    * Send HTTP response from static data, along with other header information
@@ -111,9 +112,10 @@
    * @param gzipped True if content is gzip compressed
    * @param extraHeader Extra HTTP headers to send, including final "\r\n"
    */
-  void SendStaticResponse(int code, const Twine& codeText,
-                          const Twine& contentType, StringRef content,
-                          bool gzipped, const Twine& extraHeader = Twine{});
+  virtual void SendStaticResponse(int code, const Twine& codeText,
+                                  const Twine& contentType, StringRef content,
+                                  bool gzipped,
+                                  const Twine& extraHeader = Twine{});
 
   /**
    * Send error header and message.
@@ -124,7 +126,7 @@
    * @param code HTTP error code (e.g. 404)
    * @param message Additional message text
    */
-  void SendError(int code, const Twine& message = Twine{});
+  virtual void SendError(int code, const Twine& message = Twine{});
 
   /** The HTTP request. */
   HttpParser m_request{HttpParser::kRequest};

wpiutil/src/main/native/include/wpi/HttpUtil.h

diff --git a/wpiutil/src/main/native/include/wpi/HttpUtil.h b/wpiutil/src/main/native/include/wpi/HttpUtil.h
index b0668eb..75b6e58 100644
--- a/wpiutil/src/main/native/include/wpi/HttpUtil.h
+++ b/wpiutil/src/main/native/include/wpi/HttpUtil.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -8,7 +8,9 @@
 #ifndef WPIUTIL_WPI_HTTPUTIL_H_
 #define WPIUTIL_WPI_HTTPUTIL_H_
 
+#include <initializer_list>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -59,6 +61,230 @@
 bool FindMultipartBoundary(wpi::raw_istream& is, StringRef boundary,
                            std::string* saveBuf);
 
+/**
+ * Map for looking up elements of the query portion of a URI.  Does not
+ * handle multiple elements with the same name.  This is a reference type;
+ * it does not make a copy of the query string, so it is important that the
+ * query string has a lifetime at least as long as this object.
+ */
+class HttpQueryMap {
+ public:
+  /**
+   * Constructs an empty map (with no entries).
+   */
+  HttpQueryMap() = default;
+
+  /**
+   * Constructs from an escaped query string.  Note: does not make a copy of
+   * the query string, so it must not be a temporary.
+   *
+   * @param query query string
+   */
+  explicit HttpQueryMap(StringRef query);
+
+  /**
+   * Gets an element of the query string.  Both the name and the returned
+   * value are unescaped strings.
+   *
+   * @param name name (unescaped)
+   * @param buf result buffer for value
+   * @return Optional unescaped value.  Returns an empty optional if the
+   *         name is not present in the query map.
+   */
+  std::optional<StringRef> Get(StringRef name,
+                               SmallVectorImpl<char>& buf) const;
+
+ private:
+  StringMap<StringRef> m_elems;
+};
+
+class HttpPathRef;
+
+/**
+ * Class for HTTP path matching.  A root path is represented as a single
+ * empty element, otherwise the path consists of each non-empty element
+ * between the '/' characters:
+ *  - "" -> []
+ *  - "/" -> [""]
+ *  - "/foo" -> ["foo"]
+ *  - "/foo/bar" -> ["foo", "bar"]
+ *  - "/foo//bar/" -> ["foo", "bar"]
+ *
+ * All path elements are unescaped.
+ */
+class HttpPath {
+ public:
+  /**
+   * Constructs an empty HTTP path.
+   */
+  HttpPath() = default;
+
+  /**
+   * Constructs a HTTP path from an escaped path string.  Makes a copy of the
+   * path, so it's safe to be a temporary.
+   */
+  explicit HttpPath(StringRef path);
+
+  /**
+   * Evaluates to true if the path is not empty.
+   */
+  explicit operator bool() const { return !empty(); }
+
+  /**
+   * Returns true if the path has no elements.
+   */
+  bool empty() const { return m_pathEnds.empty(); }
+
+  /**
+   * Returns number of elements in the path.
+   */
+  size_t size() const { return m_pathEnds.size(); }
+
+  /**
+   * Returns true if the path exactly matches the provided match list.
+   *
+   * @param match match list
+   * @return True if path equals match list
+   */
+  bool equals(std::initializer_list<StringRef> match) const {
+    return equals(0, makeArrayRef(match.begin(), match.end()));
+  }
+  bool equals(ArrayRef<StringRef> match) const { return equals(0, match); }
+  bool equals(StringRef match) const { return equals(0, makeArrayRef(match)); }
+
+  /**
+   * Returns true if the elements of the path starting at the "start" element
+   * match the provided match list, and there are no additional elements.
+   *
+   * @param start element to start matching at
+   * @param match match list
+   * @return True if match
+   */
+  bool equals(size_t start, std::initializer_list<StringRef> match) const {
+    return equals(start, makeArrayRef(match.begin(), match.end()));
+  }
+  bool equals(size_t start, ArrayRef<StringRef> match) const {
+    if (m_pathEnds.size() != (start + match.size())) return false;
+    return startswith(start, match);
+  }
+  bool equals(size_t start, StringRef match) const {
+    return equals(start, makeArrayRef(match));
+  }
+
+  /**
+   * Returns true if the first elements of the path match the provided match
+   * list.  The path may have additional elements.
+   *
+   * @param match match list
+   * @return True if path starts with match list
+   */
+  bool startswith(std::initializer_list<StringRef> match) const {
+    return startswith(0, makeArrayRef(match.begin(), match.end()));
+  }
+  bool startswith(ArrayRef<StringRef> match) const {
+    return startswith(0, match);
+  }
+  bool startswith(StringRef match) const {
+    return startswith(0, makeArrayRef(match));
+  }
+
+  /**
+   * Returns true if the elements of the path starting at the "start" element
+   * match the provided match list.  The path may have additional elements.
+   *
+   * @param start element to start matching at
+   * @param match match list
+   * @return True if path starting at the start element matches the match list
+   */
+  bool startswith(size_t start, std::initializer_list<StringRef> match) const {
+    return startswith(start, makeArrayRef(match.begin(), match.end()));
+  }
+
+  bool startswith(size_t start, ArrayRef<StringRef> match) const;
+
+  bool startswith(size_t start, StringRef match) const {
+    return startswith(start, makeArrayRef(match));
+  }
+
+  /**
+   * Gets a single element of the path.
+   */
+  StringRef operator[](size_t n) const {
+    return m_pathBuf.slice(n == 0 ? 0 : m_pathEnds[n - 1], m_pathEnds[n]);
+  }
+
+  /**
+   * Returns a path reference with the first N elements of the path removed.
+   */
+  HttpPathRef drop_front(size_t n) const;
+
+ private:
+  SmallString<128> m_pathBuf;
+  SmallVector<size_t, 16> m_pathEnds;
+};
+
+/**
+ * Proxy reference object for a portion of a HttpPath.
+ */
+class HttpPathRef {
+ public:
+  HttpPathRef() = default;
+  /*implicit*/ HttpPathRef(const HttpPath& path,  // NOLINT(runtime/explicit)
+                           size_t start = 0)
+      : m_path(&path), m_start(start) {}
+
+  explicit operator bool() const { return !empty(); }
+  bool empty() const { return m_path && m_path->size() == m_start; }
+  size_t size() const { return m_path ? m_path->size() - m_start : 0; }
+
+  bool equals(std::initializer_list<StringRef> match) const {
+    return equals(0, makeArrayRef(match.begin(), match.end()));
+  }
+  bool equals(ArrayRef<StringRef> match) const { return equals(0, match); }
+  bool equals(StringRef match) const { return equals(0, makeArrayRef(match)); }
+
+  bool equals(size_t start, std::initializer_list<StringRef> match) const {
+    return equals(start, makeArrayRef(match.begin(), match.end()));
+  }
+  bool equals(size_t start, ArrayRef<StringRef> match) const {
+    return m_path ? m_path->equals(m_start + start, match) : false;
+  }
+  bool equals(size_t start, StringRef match) const {
+    return equals(start, makeArrayRef(match));
+  }
+
+  bool startswith(std::initializer_list<StringRef> match) const {
+    return startswith(0, makeArrayRef(match.begin(), match.end()));
+  }
+  bool startswith(ArrayRef<StringRef> match) const {
+    return startswith(0, match);
+  }
+  bool startswith(StringRef match) const {
+    return startswith(0, makeArrayRef(match));
+  }
+
+  bool startswith(size_t start, std::initializer_list<StringRef> match) const {
+    return startswith(start, makeArrayRef(match.begin(), match.end()));
+  }
+  bool startswith(size_t start, ArrayRef<StringRef> match) const {
+    return m_path ? m_path->startswith(m_start + start, match) : false;
+  }
+  bool startswith(size_t start, StringRef match) const {
+    return startswith(start, makeArrayRef(match));
+  }
+
+  StringRef operator[](size_t n) const {
+    return m_path ? m_path->operator[](m_start + n) : StringRef{};
+  }
+  HttpPathRef drop_front(size_t n) const {
+    return m_path ? m_path->drop_front(m_start + n) : HttpPathRef{};
+  }
+
+ private:
+  const HttpPath* m_path = nullptr;
+  size_t m_start = 0;
+};
+
 class HttpLocation {
  public:
   HttpLocation() = default;

wpiutil/src/main/native/include/wpi/HttpUtil.inl

diff --git a/wpiutil/src/main/native/include/wpi/HttpUtil.inl b/wpiutil/src/main/native/include/wpi/HttpUtil.inl
index ce118b0..f66c746 100644
--- a/wpiutil/src/main/native/include/wpi/HttpUtil.inl
+++ b/wpiutil/src/main/native/include/wpi/HttpUtil.inl
@@ -1,15 +1,21 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2015. All Rights Reserved.                             */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#ifndef WPIUTIL_SUPPORT_HTTPUTIL_INL_
-#define WPIUTIL_SUPPORT_HTTPUTIL_INL_
+#ifndef WPIUTIL_WPI_HTTPUTIL_INL_
+#define WPIUTIL_WPI_HTTPUTIL_INL_
+
+#include <utility>
 
 namespace wpi {
 
+inline HttpPathRef HttpPath::drop_front(size_t n) const {
+  return HttpPathRef(*this, n);
+}
+
 template <typename T>
 HttpRequest::HttpRequest(const HttpLocation& loc, const T& extraParams)
     : host{loc.host}, port{loc.port} {
@@ -45,4 +51,4 @@
 
 }  // namespace wpi
 
-#endif  // WPIUTIL_SUPPORT_HTTPUTIL_INL_
+#endif  // WPIUTIL_WPI_HTTPUTIL_INL_

wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h

diff --git a/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h b/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h
new file mode 100644
index 0000000..d662471
--- /dev/null
+++ b/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#ifndef WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_H_
+#define WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_H_
+
+#include <initializer_list>
+#include <memory>
+#include <string>
+
+#include "wpi/ArrayRef.h"
+#include "wpi/HttpServerConnection.h"
+#include "wpi/SmallVector.h"
+#include "wpi/StringRef.h"
+#include "wpi/WebSocket.h"
+#include "wpi/WebSocketServer.h"
+#include "wpi/uv/Stream.h"
+
+namespace wpi {
+
+/**
+ * A server-side HTTP connection that also accepts WebSocket upgrades.
+ *
+ * @tparam Derived derived class for std::enable_shared_from_this.
+ */
+template <typename Derived>
+class HttpWebSocketServerConnection
+    : public HttpServerConnection,
+      public std::enable_shared_from_this<Derived> {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param stream network stream
+   * @param protocols Acceptable subprotocols
+   */
+  HttpWebSocketServerConnection(std::shared_ptr<uv::Stream> stream,
+                                ArrayRef<StringRef> protocols);
+
+  /**
+   * Constructor.
+   *
+   * @param stream network stream
+   * @param protocols Acceptable subprotocols
+   */
+  HttpWebSocketServerConnection(std::shared_ptr<uv::Stream> stream,
+                                std::initializer_list<StringRef> protocols)
+      : HttpWebSocketServerConnection(
+            stream, makeArrayRef(protocols.begin(), protocols.end())) {}
+
+ protected:
+  /**
+   * Check that an incoming WebSocket upgrade is okay.  This is called prior
+   * to accepting the upgrade (so prior to ProcessWsUpgrade()).
+   *
+   * The implementation should check other headers and return true if the
+   * WebSocket connection should be accepted.
+   *
+   * @param protocol negotiated subprotocol
+   */
+  virtual bool IsValidWsUpgrade(StringRef protocol) { return true; }
+
+  /**
+   * Process an incoming WebSocket upgrade.  This is called after the header
+   * reader has been disconnected and the websocket has been accepted.
+   *
+   * The implementation should set up appropriate callbacks on the websocket
+   * object to continue communication.
+   *
+   * @note When a WebSocket upgrade occurs, the stream user data is replaced
+   *       with the websocket, and the websocket user data points to "this".
+   *       Replace the websocket user data with caution!
+   */
+  virtual void ProcessWsUpgrade() = 0;
+
+  /**
+   * WebSocket connection; not valid until ProcessWsUpgrade is called.
+   */
+  WebSocket* m_websocket = nullptr;
+
+ private:
+  WebSocketServerHelper m_helper;
+  SmallVector<std::string, 2> m_protocols;
+};
+
+}  // namespace wpi
+
+#include "HttpWebSocketServerConnection.inl"
+
+#endif  // WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_H_

wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inl

diff --git a/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inl b/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inl
new file mode 100644
index 0000000..586f189
--- /dev/null
+++ b/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inl
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#ifndef WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INL_
+#define WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INL_
+
+#include <memory>
+
+namespace wpi {
+
+template <typename Derived>
+HttpWebSocketServerConnection<Derived>::HttpWebSocketServerConnection(
+    std::shared_ptr<uv::Stream> stream, ArrayRef<StringRef> protocols)
+    : HttpServerConnection{stream},
+      m_helper{m_request},
+      m_protocols{protocols.begin(), protocols.end()} {
+  // Handle upgrade event
+  m_helper.upgrade.connect([this] {
+    // Negotiate sub-protocol
+    SmallVector<StringRef, 2> protocols{m_protocols.begin(), m_protocols.end()};
+    StringRef protocol = m_helper.MatchProtocol(protocols).second;
+
+    // Check that the upgrade is valid
+    if (!IsValidWsUpgrade(protocol)) return;
+
+    // Disconnect HttpServerConnection header reader
+    m_dataConn.disconnect();
+    m_messageCompleteConn.disconnect();
+
+    // Accepting the stream may destroy this (as it replaces the stream user
+    // data), so grab a shared pointer first.
+    auto self = this->shared_from_this();
+
+    // Accept the upgrade
+    auto ws = m_helper.Accept(m_stream, protocol);
+
+    // Set this as the websocket user data to keep it around
+    ws->SetData(self);
+
+    // Store in member
+    m_websocket = ws.get();
+
+    // Call derived class function
+    ProcessWsUpgrade();
+  });
+}
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INL_

wpiutil/src/main/native/include/wpi/MimeTypes.h

diff --git a/wpiutil/src/main/native/include/wpi/MimeTypes.h b/wpiutil/src/main/native/include/wpi/MimeTypes.h
new file mode 100644
index 0000000..b3dc17b
--- /dev/null
+++ b/wpiutil/src/main/native/include/wpi/MimeTypes.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#ifndef WPIUTIL_WPI_MIMETYPES_H_
+#define WPIUTIL_WPI_MIMETYPES_H_
+
+#include "wpi/StringRef.h"
+
+namespace wpi {
+
+StringRef MimeTypeFromPath(StringRef path);
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_MIMETYPES_H_

wpiutil/src/main/native/include/wpi/PriorityQueue.h

diff --git a/wpiutil/src/main/native/include/wpi/PriorityQueue.h b/wpiutil/src/main/native/include/wpi/PriorityQueue.h
deleted file mode 100644
index 37585a3..0000000
--- a/wpiutil/src/main/native/include/wpi/PriorityQueue.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef WPIUTIL_WPI_PRIORITYQUEUE_H_
-#define WPIUTIL_WPI_PRIORITYQUEUE_H_
-
-#include <algorithm>
-#include <functional>
-#include <queue>
-#include <vector>
-
-namespace wpi {
-
-/**
- * This class adds a method for removing all elements from the priority queue
- * matching the given value.
- */
-template <class T, class Container = std::vector<T>,
-          class Compare = std::less<typename Container::value_type>>
-class PriorityQueue : public std::priority_queue<T, Container, Compare> {
- public:
-  bool remove(const T& value) {
-    auto it = std::find(this->c.begin(), this->c.end(), value);
-    if (it != this->c.end()) {
-      this->c.erase(it);
-      std::make_heap(this->c.begin(), this->c.end(), this->comp);
-      return true;
-    } else {
-      return false;
-    }
-  }
-};
-
-}  // namespace wpi
-
-#endif  // WPIUTIL_WPI_PRIORITYQUEUE_H_

wpiutil/src/main/native/include/wpi/Signal.h

diff --git a/wpiutil/src/main/native/include/wpi/Signal.h b/wpiutil/src/main/native/include/wpi/Signal.h
index d8109ff..64b774c 100644
--- a/wpiutil/src/main/native/include/wpi/Signal.h
+++ b/wpiutil/src/main/native/include/wpi/Signal.h
@@ -815,7 +815,7 @@
  *
  * Beware of accidentally using recursive signal emission or cycles between
  * two or more signals in your code. Locking std::mutex more than once is
- * undefined behaviour, even if it "seems to work somehow". Use signal_r
+ * undefined behavior, even if it "seems to work somehow". Use signal_r
  * instead for that use case.
  */
 template <typename... T>

wpiutil/src/main/native/include/wpi/TCPStream.h

diff --git a/wpiutil/src/main/native/include/wpi/TCPStream.h b/wpiutil/src/main/native/include/wpi/TCPStream.h
index 6a38ffb..0c6e8a9 100644
--- a/wpiutil/src/main/native/include/wpi/TCPStream.h
+++ b/wpiutil/src/main/native/include/wpi/TCPStream.h
@@ -1,7 +1,7 @@
 /*
    TCPStream.h
 
-   TCPStream class interface. TCPStream provides methods to trasnfer
+   TCPStream class interface. TCPStream provides methods to transfer
    data between peers over a TCP/IP connection.
 
    ------------------------------------------

wpiutil/src/main/native/include/wpi/circular_buffer.h

diff --git a/wpiutil/src/main/native/include/wpi/circular_buffer.h b/wpiutil/src/main/native/include/wpi/circular_buffer.h
index 097d5f6..5dc12f8 100644
--- a/wpiutil/src/main/native/include/wpi/circular_buffer.h
+++ b/wpiutil/src/main/native/include/wpi/circular_buffer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -47,6 +47,8 @@
  private:
   std::vector<T> m_data;
 
+  T zero_val{0};
+
   // Index of element at front of buffer
   size_t m_front = 0;
 

wpiutil/src/main/native/include/wpi/circular_buffer.inc

diff --git a/wpiutil/src/main/native/include/wpi/circular_buffer.inc b/wpiutil/src/main/native/include/wpi/circular_buffer.inc
index 7f029e4..20690b7 100644
--- a/wpiutil/src/main/native/include/wpi/circular_buffer.inc
+++ b/wpiutil/src/main/native/include/wpi/circular_buffer.inc
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,7 +12,7 @@
 namespace wpi {
 
 template <class T>
-circular_buffer<T>::circular_buffer(size_t size) : m_data(size, 0) {}
+circular_buffer<T>::circular_buffer(size_t size) : m_data(size, T{0}) {}
 
 /**
  * Returns number of elements in buffer
@@ -45,7 +45,7 @@
 T& circular_buffer<T>::back() {
   // If there are no elements in the buffer, do nothing
   if (m_length == 0) {
-    return 0;
+    return zero_val;
   }
 
   return m_data[(m_front + m_length - 1) % m_data.size()];
@@ -58,7 +58,7 @@
 const T& circular_buffer<T>::back() const {
   // If there are no elements in the buffer, do nothing
   if (m_length == 0) {
-    return 0;
+    return zero_val;
   }
 
   return m_data[(m_front + m_length - 1) % m_data.size()];
@@ -110,7 +110,7 @@
 T circular_buffer<T>::pop_front() {
   // If there are no elements in the buffer, do nothing
   if (m_length == 0) {
-    return 0;
+    return T{0};
   }
 
   T& temp = m_data[m_front];
@@ -126,7 +126,7 @@
 T circular_buffer<T>::pop_back() {
   // If there are no elements in the buffer, do nothing
   if (m_length == 0) {
-    return 0;
+    return T{0};
   }
 
   m_length--;
@@ -191,7 +191,7 @@
  */
 template <class T>
 void circular_buffer<T>::reset() {
-  std::fill(m_data.begin(), m_data.end(), 0);
+  std::fill(m_data.begin(), m_data.end(), T{0});
   m_front = 0;
   m_length = 0;
 }

wpiutil/src/main/native/include/wpi/http_parser.h

diff --git a/wpiutil/src/main/native/include/wpi/http_parser.h b/wpiutil/src/main/native/include/wpi/http_parser.h
index f4bf9fd..2189b8f 100644
--- a/wpiutil/src/main/native/include/wpi/http_parser.h
+++ b/wpiutil/src/main/native/include/wpi/http_parser.h
@@ -63,7 +63,7 @@
  * chunked' headers that indicate the presence of a body.
  *
  * Returning `2` from on_headers_complete will tell parser that it should not
- * expect neither a body nor any futher responses on this connection. This is
+ * expect neither a body nor any further responses on this connection. This is
  * useful for handling responses to a CONNECT request which may not contain
  * `Upgrade` or `Connection: upgrade` headers.
  *

wpiutil/src/main/native/include/wpi/priority_queue.h

diff --git a/wpiutil/src/main/native/include/wpi/priority_queue.h b/wpiutil/src/main/native/include/wpi/priority_queue.h
new file mode 100644
index 0000000..b194090
--- /dev/null
+++ b/wpiutil/src/main/native/include/wpi/priority_queue.h
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#ifndef WPIUTIL_WPI_PRIORITY_QUEUE_H_
+#define WPIUTIL_WPI_PRIORITY_QUEUE_H_
+
+#include <algorithm>
+#include <functional>
+#include <utility>
+#include <vector>
+
+namespace wpi {
+
+/**
+ * This class is the same as std::priority_queue with two changes:
+ *
+ * 1. Adds a remove() function for removing all elements from the priority queue
+ *    that match the given value.
+ * 2. Replaces "void pop()" with "T pop()" so the element can be moved from the
+ *    queue directly instead of copied from top().
+ */
+template <typename T, typename Sequence = std::vector<T>,
+          typename Compare = std::less<typename Sequence::value_type>>
+class priority_queue {
+ public:
+  static_assert(std::is_same_v<T, typename Sequence::value_type>,
+                "value_type must be the same as the underlying container");
+
+  using value_type = typename Sequence::value_type;
+  using reference = typename Sequence::reference;
+  using const_reference = typename Sequence::const_reference;
+  using size_type = typename Sequence::size_type;
+  using container_type = Sequence;
+  using value_compare = Compare;
+
+  template <typename Seq = Sequence,
+            typename Requires = typename std::enable_if_t<
+                std::is_default_constructible<Compare>{} &&
+                std::is_default_constructible<Seq>{}>>
+  priority_queue() {}
+
+  priority_queue(const Compare& comp, const Sequence& c) : c(c), comp(comp) {
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  explicit priority_queue(const Compare& comp, Sequence&& c = Sequence{})
+      : c(std::move(c)), comp(comp) {
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  template <typename InputIterator>
+  priority_queue(InputIterator first, InputIterator last, const Compare& comp,
+                 const Sequence& c)
+      : c(c), comp(comp) {
+    c.insert(c.end(), first, last);
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  template <typename InputIterator>
+  priority_queue(InputIterator first, InputIterator last,
+                 const Compare& comp = Compare{}, Sequence&& c = Sequence{})
+      : c(std::move(c)), comp(comp) {
+    c.insert(c.end(), first, last);
+    std::make_heap(c.begin(), c.end(), comp);
+  }
+
+  [[nodiscard]] bool empty() const { return c.empty(); }
+
+  size_type size() const { return c.size(); }
+
+  const_reference top() const { return c.front(); }
+
+  void push(const value_type& value) {
+    c.push_back(value);
+    std::push_heap(c.begin(), c.end(), comp);
+  }
+
+  void push(value_type&& value) {
+    c.push_back(std::move(value));
+    std::push_heap(c.begin(), c.end(), comp);
+  }
+
+  template <typename... Args>
+  void emplace(Args&&... args) {
+    c.emplace_back(std::forward<Args>(args)...);
+    std::push_heap(c.begin(), c.end(), comp);
+  }
+
+  T pop() {
+    std::pop_heap(c.begin(), c.end(), comp);
+    auto ret = std::move(c.back());
+    c.pop_back();
+    return ret;
+  }
+
+  bool remove(const T& value) {
+    auto it = std::find(c.begin(), c.end(), value);
+    if (it != this->c.end()) {
+      c.erase(it);
+      std::make_heap(c.begin(), c.end(), comp);
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+ protected:
+  Sequence c;
+  Compare comp;
+};
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_PRIORITY_QUEUE_H_

wpiutil/src/main/native/include/wpi/static_circular_buffer.h

diff --git a/wpiutil/src/main/native/include/wpi/static_circular_buffer.h b/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
new file mode 100644
index 0000000..8329917
--- /dev/null
+++ b/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
@@ -0,0 +1,276 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <cstddef>
+
+namespace wpi {
+
+/**
+ * This is a simple circular buffer so we don't need to "bucket brigade" copy
+ * old values.
+ */
+template <class T, size_t N>
+class static_circular_buffer {
+ public:
+  static_assert(N > 0, "The circular buffer size shouldn't be zero.");
+
+  constexpr static_circular_buffer() = default;
+
+  class iterator {
+   public:
+    using iterator_category = std::forward_iterator_tag;
+    using value_type = T;
+    using difference_type = std::ptrdiff_t;
+    using pointer = T*;
+    using reference = T&;
+
+    iterator(static_circular_buffer* buffer, size_t index)
+        : m_buffer(buffer), m_index(index) {}
+
+    iterator& operator++() {
+      ++m_index;
+      return *this;
+    }
+    iterator operator++(int) {
+      iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    bool operator==(const iterator& other) const {
+      return m_buffer == other.m_buffer && m_index == other.m_index;
+    }
+    bool operator!=(const iterator& other) const { return !(*this == other); }
+    reference operator*() { return (*m_buffer)[m_index]; }
+
+   private:
+    static_circular_buffer* m_buffer;
+    size_t m_index;
+  };
+
+  class const_iterator {
+   public:
+    using iterator_category = std::forward_iterator_tag;
+    using value_type = T;
+    using difference_type = std::ptrdiff_t;
+    using pointer = T*;
+    using const_reference = const T&;
+
+    const_iterator(const static_circular_buffer* buffer, size_t index)
+        : m_buffer(buffer), m_index(index) {}
+
+    const_iterator& operator++() {
+      ++m_index;
+      return *this;
+    }
+    const_iterator operator++(int) {
+      const_iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    bool operator==(const const_iterator& other) const {
+      return m_buffer == other.m_buffer && m_index == other.m_index;
+    }
+    bool operator!=(const const_iterator& other) const {
+      return !(*this == other);
+    }
+    const_reference operator*() const { return (*m_buffer)[m_index]; }
+
+   private:
+    const static_circular_buffer* m_buffer;
+    size_t m_index;
+  };
+
+  iterator begin() { return iterator(this, 0); }
+  iterator end() {
+    return iterator(this, ::wpi::static_circular_buffer<T, N>::size());
+  }
+
+  const_iterator begin() const { return const_iterator(this, 0); }
+  const_iterator end() const {
+    return const_iterator(this, ::wpi::static_circular_buffer<T, N>::size());
+  }
+
+  const_iterator cbegin() const { return const_iterator(this, 0); }
+  const_iterator cend() const {
+    return const_iterator(this, ::wpi::static_circular_buffer<T, N>::size());
+  }
+
+  /**
+   * Returns number of elements in buffer
+   */
+  size_t size() const { return m_length; }
+
+  /**
+   * Returns value at front of buffer
+   */
+  T& front() { return (*this)[0]; }
+
+  /**
+   * Returns value at front of buffer
+   */
+  const T& front() const { return (*this)[0]; }
+
+  /**
+   * Returns value at back of buffer
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  T& back() { return m_data[(m_front + m_length - 1) % N]; }
+
+  /**
+   * Returns value at back of buffer
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  const T& back() const { return m_data[(m_front + m_length - 1) % N]; }
+
+  /**
+   * Push a new value onto the front of the buffer.
+   *
+   * The value at the back is overwritten if the buffer is full.
+   */
+  void push_front(T value) {
+    m_front = ModuloDec(m_front);
+
+    m_data[m_front] = value;
+
+    if (m_length < N) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push a new value onto the back of the buffer.
+   *
+   * The value at the front is overwritten if the buffer is full.
+   */
+  void push_back(T value) {
+    m_data[(m_front + m_length) % N] = value;
+
+    if (m_length < N) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = ModuloInc(m_front);
+    }
+  }
+
+  /**
+   * Push a new value onto the front of the buffer that is constructed with the
+   * provided constructor arguments.
+   *
+   * The value at the back is overwritten if the buffer is full.
+   */
+  template <class... Args>
+  void emplace_front(Args&&... args) {
+    m_front = ModuloDec(m_front);
+
+    m_data[m_front] = T{args...};
+
+    if (m_length < N) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push a new value onto the back of the buffer that is constructed with the
+   * provided constructor arguments.
+   *
+   * The value at the front is overwritten if the buffer is full.
+   */
+  template <class... Args>
+  void emplace_back(Args&&... args) {
+    m_data[(m_front + m_length) % N] = T{args...};
+
+    if (m_length < N) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = ModuloInc(m_front);
+    }
+  }
+
+  /**
+   * Pop value at front of buffer.
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  T pop_front() {
+    T& temp = m_data[m_front];
+    m_front = ModuloInc(m_front);
+    m_length--;
+    return temp;
+  }
+
+  /**
+   * Pop value at back of buffer.
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  T pop_back() {
+    m_length--;
+    return m_data[(m_front + m_length) % N];
+  }
+
+  /**
+   * Empties internal buffer.
+   */
+  void reset() {
+    m_front = 0;
+    m_length = 0;
+  }
+
+  /**
+   * @return Element at index starting from front of buffer.
+   */
+  T& operator[](size_t index) { return m_data[(m_front + index) % N]; }
+
+  /**
+   * @return Element at index starting from front of buffer.
+   */
+  const T& operator[](size_t index) const {
+    return m_data[(m_front + index) % N];
+  }
+
+ private:
+  std::array<T, N> m_data;
+
+  // Index of element at front of buffer
+  size_t m_front = 0;
+
+  // Number of elements used in buffer
+  size_t m_length = 0;
+
+  /**
+   * Increment an index modulo the length of the buffer.
+   *
+   * @return The result of the modulo operation.
+   */
+  size_t ModuloInc(size_t index) { return (index + 1) % N; }
+
+  /**
+   * Decrement an index modulo the length of the buffer.
+   *
+   * @return The result of the modulo operation.
+   */
+  size_t ModuloDec(size_t index) {
+    if (index == 0) {
+      return N - 1;
+    } else {
+      return index - 1;
+    }
+  }
+};
+
+}  // namespace wpi

wpiutil/src/main/native/include/wpi/uv/Buffer.h

diff --git a/wpiutil/src/main/native/include/wpi/uv/Buffer.h b/wpiutil/src/main/native/include/wpi/uv/Buffer.h
index 51acd76..04e96b9 100644
--- a/wpiutil/src/main/native/include/wpi/uv/Buffer.h
+++ b/wpiutil/src/main/native/include/wpi/uv/Buffer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -107,6 +107,7 @@
    * @param size Size of each buffer to allocate.
    */
   explicit SimpleBufferPool(size_t size = 4096) : m_size{size} {}
+  ~SimpleBufferPool() { Clear(); }
 
   SimpleBufferPool(const SimpleBufferPool& other) = delete;
   SimpleBufferPool& operator=(const SimpleBufferPool& other) = delete;

wpiutil/src/main/native/include/wpi/uv/util.h

diff --git a/wpiutil/src/main/native/include/wpi/uv/util.h b/wpiutil/src/main/native/include/wpi/uv/util.h
index 3795aa8..14a021d 100644
--- a/wpiutil/src/main/native/include/wpi/uv/util.h
+++ b/wpiutil/src/main/native/include/wpi/uv/util.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -58,6 +58,24 @@
 }
 
 /**
+ * Convert a binary structure containing an IPv4 or IPv6 address to a string.
+ * @param addr Binary structure
+ * @param ip Output string (any type that has `assign(char*, char*)`)
+ * @param port Output port number
+ * @return Error (same as `uv_ip6_name()`).
+ */
+template <typename T>
+int AddrToName(const sockaddr_storage& addr, T* ip, unsigned int* port) {
+  if (addr.ss_family == AF_INET)
+    return AddrToName(reinterpret_cast<const sockaddr_in&>(addr), ip, port);
+  if (addr.ss_family == AF_INET6)
+    return AddrToName(reinterpret_cast<const sockaddr_in6&>(addr), ip, port);
+  char name[1];
+  ip->assign(name, name);
+  return -1;
+}
+
+/**
  * Convert a binary IPv4 address to a string.
  * @param addr Binary address
  * @param ip Output string (any type that has `assign(char*, char*)`)

wpiutil/src/main/native/libuv/src/unix/tty.cpp

diff --git a/wpiutil/src/main/native/libuv/src/unix/tty.cpp b/wpiutil/src/main/native/libuv/src/unix/tty.cpp
index 74d3d75..5f68140 100644
--- a/wpiutil/src/main/native/libuv/src/unix/tty.cpp
+++ b/wpiutil/src/main/native/libuv/src/unix/tty.cpp
@@ -38,7 +38,7 @@
 static struct termios orig_termios;
 static uv_spinlock_t termios_spinlock = UV_SPINLOCK_INITIALIZER;
 
-static int uv__tty_is_slave(const int fd) {
+static int uv__tty_is_peripheral(const int fd) {
   int result;
 #if defined(__linux__) || defined(__FreeBSD__) || defined(__FreeBSD_kernel__)
   int dummy;
@@ -50,15 +50,16 @@
   result = ioctl(fd, TIOCPTYGNAME, &dummy) != 0;
 #elif defined(__NetBSD__)
   /*
-   * NetBSD as an extension returns with ptsname(3) and ptsname_r(3) the slave
-   * device name for both descriptors, the master one and slave one.
+   * NetBSD as an extension returns with ptsname(3) and ptsname_r(3) the
+   * peripheral device name for both descriptors, the controller one and
+   * peripheral one.
    *
    * Implement function to compare major device number with pts devices.
    *
    * The major numbers are machine-dependent, on NetBSD/amd64 they are
    * respectively:
-   *  - master tty: ptc - major 6
-   *  - slave tty:  pts - major 5
+   *  - controller tty: ptc - major 6
+   *  - peripheral tty:  pts - major 5
    */
 
   struct stat sb;
@@ -133,12 +134,12 @@
    * other processes.
    */
   if (type == UV_TTY) {
-    /* Reopening a pty in master mode won't work either because the reopened
-     * pty will be in slave mode (*BSD) or reopening will allocate a new
-     * master/slave pair (Linux). Therefore check if the fd points to a
-     * slave device.
+    /* Reopening a pty in controller mode won't work either because the reopened
+     * pty will be in peripheral mode (*BSD) or reopening will allocate a new
+     * controller/peripheral pair (Linux). Therefore check if the fd points to a
+     * peripheral device.
      */
-    if (uv__tty_is_slave(fd) && ttyname_r(fd, path, sizeof(path)) == 0)
+    if (uv__tty_is_peripheral(fd) && ttyname_r(fd, path, sizeof(path)) == 0)
       r = uv__open_cloexec(path, mode);
     else
       r = -1;

wpiutil/src/main/native/unix/Demangle.cpp

diff --git a/wpiutil/src/main/native/unix/Demangle.cpp b/wpiutil/src/main/native/unix/Demangle.cpp
index 1ce4f31..3453084 100644
--- a/wpiutil/src/main/native/unix/Demangle.cpp
+++ b/wpiutil/src/main/native/unix/Demangle.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,26 +11,26 @@
 
 #include <cstdio>
 
+#include "wpi/SmallString.h"
+
 namespace wpi {
 
-std::string Demangle(char const* mangledSymbol) {
-  char buffer[256];
+std::string Demangle(const Twine& mangledSymbol) {
+  SmallString<128> buf;
   size_t length;
   int32_t status;
 
-  if (std::sscanf(mangledSymbol, "%*[^(]%*[(]%255[^)+]", buffer)) {
-    char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
-    if (status == 0) {
-      return symbol;
-    } else {
-      // If the symbol couldn't be demangled, it's probably a C function,
-      // so just return it as-is.
-      return buffer;
-    }
+  char* symbol =
+      abi::__cxa_demangle(mangledSymbol.toNullTerminatedStringRef(buf).data(),
+                          nullptr, &length, &status);
+  if (status == 0) {
+    std::string rv{symbol};
+    std::free(symbol);
+    return rv;
   }
 
   // If everything else failed, just return the mangled symbol
-  return mangledSymbol;
+  return mangledSymbol.str();
 }
 
 }  // namespace wpi

wpiutil/src/main/native/unix/StackTrace.cpp

diff --git a/wpiutil/src/main/native/unix/StackTrace.cpp b/wpiutil/src/main/native/unix/StackTrace.cpp
index be28080..004eb39 100644
--- a/wpiutil/src/main/native/unix/StackTrace.cpp
+++ b/wpiutil/src/main/native/unix/StackTrace.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,6 +11,7 @@
 
 #include "wpi/Demangle.h"
 #include "wpi/SmallString.h"
+#include "wpi/StringRef.h"
 #include "wpi/raw_ostream.h"
 
 namespace wpi {
@@ -25,7 +26,11 @@
   for (int i = offset; i < stackSize; i++) {
     // Only print recursive functions once in a row.
     if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
-      trace << "\tat " << Demangle(mangledSymbols[i]) << "\n";
+      // extract just function name from "pathToExe(functionName+offset)"
+      StringRef sym{mangledSymbols[i]};
+      sym = sym.split('(').second;
+      sym = sym.split('+').first;
+      trace << "\tat " << Demangle(sym) << "\n";
     }
   }
 

wpiutil/src/main/native/windows/Demangle.cpp

diff --git a/wpiutil/src/main/native/windows/Demangle.cpp b/wpiutil/src/main/native/windows/Demangle.cpp
index 34d1cab..a1560d6 100644
--- a/wpiutil/src/main/native/windows/Demangle.cpp
+++ b/wpiutil/src/main/native/windows/Demangle.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -11,19 +11,22 @@
 
 #include <dbghelp.h>
 
+#include "wpi/SmallString.h"
 #include "wpi/mutex.h"
 
 #pragma comment(lib, "Dbghelp.lib")
 
 namespace wpi {
 
-std::string Demangle(char const* mangledSymbol) {
+std::string Demangle(const Twine& mangledSymbol) {
   static wpi::mutex m;
   std::scoped_lock lock(m);
+  SmallString<128> buf;
   char buffer[256];
-  DWORD sz = UnDecorateSymbolName(mangledSymbol, buffer, sizeof(buffer),
-                                  UNDNAME_COMPLETE);
-  if (sz == 0) return mangledSymbol;
+  DWORD sz =
+      UnDecorateSymbolName(mangledSymbol.toNullTerminatedStringRef(buf).data(),
+                           buffer, sizeof(buffer), UNDNAME_COMPLETE);
+  if (sz == 0) return mangledSymbol.str();
   return std::string(buffer, sz);
 }
 

wpiutil/src/netconsoleTee/native/cpp/main.cpp

diff --git a/wpiutil/src/netconsoleTee/native/cpp/main.cpp b/wpiutil/src/netconsoleTee/native/cpp/main.cpp
index 6c2a4fe..82cc82a 100644
--- a/wpiutil/src/netconsoleTee/native/cpp/main.cpp
+++ b/wpiutil/src/netconsoleTee/native/cpp/main.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -57,14 +57,14 @@
   return true;
 }
 
-static void CopyUdp(uv::Stream& in, std::shared_ptr<uv::Udp> out,
+static void CopyUdp(uv::Stream& in, std::shared_ptr<uv::Udp> out, int port,
                     bool broadcast) {
   sockaddr_in addr;
   if (broadcast) {
     out->SetBroadcast(true);
-    uv::NameToAddr("0.0.0.0", 6666, &addr);
+    uv::NameToAddr("0.0.0.0", port, &addr);
   } else {
-    uv::NameToAddr("127.0.0.1", 6666, &addr);
+    uv::NameToAddr("127.0.0.1", port, &addr);
   }
 
   in.data.connect(
@@ -119,6 +119,7 @@
   bool useUdp = false;
   bool broadcastUdp = false;
   bool err = false;
+  int port = -1;
 
   while (arg < argc && argv[arg][0] == '-') {
     if (wpi::StringRef(argv[arg]) == "-u") {
@@ -126,6 +127,13 @@
     } else if (wpi::StringRef(argv[arg]) == "-b") {
       useUdp = true;
       broadcastUdp = true;
+    } else if (wpi::StringRef(argv[arg]) == "-p") {
+      ++arg;
+      if (arg >= argc || argv[arg][0] == '-' ||
+          wpi::StringRef(argv[arg]).getAsInteger(10, port)) {
+        wpi::errs() << "-p must be followed by port number\n";
+        err = true;
+      }
     } else {
       wpi::errs() << "unrecognized command line option " << argv[arg] << '\n';
       err = true;
@@ -135,9 +143,10 @@
 
   if (err) {
     wpi::errs()
-        << argv[0] << " [-ub]\n"
-        << "  -u  send udp to localhost port 6666 instead of using tcp\n"
-        << "  -b  broadcast udp to port 6666 instead of using tcp\n";
+        << argv[0] << " [-ub] [-p PORT]\n"
+        << "  -u       send udp to localhost port 6666 instead of using tcp\n"
+        << "  -b       broadcast udp to port 6666 instead of using tcp\n"
+        << "  -p PORT  use port PORT instead of 6666 (udp) or 1740 (tcp)\n";
     return EXIT_FAILURE;
   }
 
@@ -161,12 +170,12 @@
   if (useUdp) {
     auto udp = uv::Udp::Create(loop);
     // tee
-    CopyUdp(*stdinTty, udp, broadcastUdp);
+    CopyUdp(*stdinTty, udp, port < 0 ? 6666 : port, broadcastUdp);
   } else {
     auto tcp = uv::Tcp::Create(loop);
 
     // bind to listen address and port
-    tcp->Bind("", 1740);
+    tcp->Bind("", port < 0 ? 1740 : port);
 
     // when we get a connection, accept it
     tcp->connection.connect([srv = tcp.get(), stdinTty] {

wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java

diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java b/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java
new file mode 100644
index 0000000..e92efe4
--- /dev/null
+++ b/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpiutil;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+public class WPIUtilJNITest {
+  @Test
+  public void testNow() {
+    assertDoesNotThrow(WPIUtilJNI::now);
+  }
+}

wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java

diff --git a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java b/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java
deleted file mode 100644
index 4d6697d..0000000
--- a/wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java
+++ /dev/null
@@ -1,210 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import org.ejml.data.SingularMatrixException;
-import org.ejml.dense.row.MatrixFeatures_DDRM;
-import org.ejml.simple.SimpleMatrix;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N3;
-import edu.wpi.first.wpiutil.math.numbers.N4;
-
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class MatrixTest {
-  @Test
-  void testMatrixMultiplication() {
-    var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(2.0, 1.0,
-            0.0, 1.0);
-    var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(3.0, 0.0,
-            0.0, 2.5);
-
-    Matrix<N2, N2> result = mat1.times(mat2);
-
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(6.0, 2.5,
-            0.0, 2.5).getStorage().getDDRM(),
-        result.getStorage().getDDRM()
-    ));
-
-    var mat3 = MatrixUtils.mat(Nat.N2(), Nat.N3())
-        .fill(1.0, 3.0, 0.5,
-            2.0, 4.3, 1.2);
-    var mat4 = MatrixUtils.mat(Nat.N3(), Nat.N4())
-        .fill(3.0, 1.5, 2.0, 4.5,
-            2.3, 1.0, 1.6, 3.1,
-            5.2, 2.1, 2.0, 1.0);
-
-    Matrix<N2, N4> result2 = mat3.times(mat4);
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-        MatrixUtils.mat(Nat.N2(), Nat.N4())
-        .fill(12.5, 5.55, 7.8, 14.3,
-            22.13, 9.82, 13.28, 23.53).getStorage().getDDRM(),
-        result2.getStorage().getDDRM(),
-        1E-9
-    ));
-  }
-
-  @Test
-  void testMatrixVectorMultiplication() {
-    var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(1.0, 1.0,
-            0.0, 1.0);
-
-    var vec = MatrixUtils.vec(Nat.N2())
-        .fill(3.0,
-            2.0);
-
-    Matrix<N2, N1> result = mat.times(vec);
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.vec(Nat.N2())
-        .fill(5.0,
-            2.0).getStorage().getDDRM(),
-        result.getStorage().getDDRM()
-    ));
-  }
-
-  @Test
-  void testTranspose() {
-    Matrix<N3, N1> vec = MatrixUtils.vec(Nat.N3())
-        .fill(1.0,
-            2.0,
-            3.0);
-
-    Matrix<N1, N3> transpose = vec.transpose();
-
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0).getStorage()
-        .getDDRM(),
-        transpose.getStorage().getDDRM()
-    ));
-  }
-
-  @Test
-  void testInverse() {
-    var mat = MatrixUtils.mat(Nat.N3(), Nat.N3())
-        .fill(1.0, 3.0, 2.0,
-            5.0, 2.0, 1.5,
-            0.0, 1.3, 2.5);
-
-    var inv = mat.inv();
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-        MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
-        mat.times(inv).getStorage().getDDRM(),
-        1E-9
-    ));
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-        MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
-        inv.times(mat).getStorage().getDDRM(),
-        1E-9
-    ));
-  }
-
-  @Test
-  void testUninvertableMatrix() {
-    var singularMatrix = MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(2.0, 1.0,
-            2.0, 1.0);
-
-    assertThrows(SingularMatrixException.class, singularMatrix::inv);
-  }
-
-  @Test
-  void testMatrixScalarArithmetic() {
-    var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(1.0, 2.0,
-            3.0, 4.0);
-
-
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(3.0, 4.0,
-            5.0, 6.0).getStorage().getDDRM(),
-        mat.plus(2.0).getStorage().getDDRM()
-    ));
-
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(0.0, 1.0,
-            2.0, 3.0).getStorage().getDDRM(),
-        mat.minus(1.0).getStorage().getDDRM()
-    ));
-
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(2.0, 4.0,
-            6.0, 8.0).getStorage().getDDRM(),
-        mat.times(2.0).getStorage().getDDRM()
-    ));
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-        MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(0.5, 1.0,
-            1.5, 2.0).getStorage().getDDRM(),
-        mat.div(2.0).getStorage().getDDRM(),
-        1E-3
-    ));
-  }
-
-  @Test
-  void testMatrixMatrixArithmetic() {
-    var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(1.0, 2.0,
-            3.0, 4.0);
-
-    var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(5.0, 6.0,
-            7.0, 8.0);
-
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(-4.0, -4.0,
-            -4.0, -4.0).getStorage().getDDRM(),
-        mat1.minus(mat2).getStorage().getDDRM()
-    ));
-
-    assertTrue(MatrixFeatures_DDRM.isEquals(
-        MatrixUtils.mat(Nat.N2(), Nat.N2())
-        .fill(6.0, 8.0,
-            10.0, 12.0).getStorage().getDDRM(),
-        mat1.plus(mat2).getStorage().getDDRM()
-    ));
-  }
-
-  @Test
-  void testMatrixExponential() {
-    SimpleMatrix matrix = MatrixUtils.eye(Nat.N2()).getStorage();
-    var result = SimpleMatrixUtils.expm(matrix);
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-        result.getDDRM(),
-        new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
-        1E-9
-    ));
-
-    matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4});
-    result = SimpleMatrixUtils.expm(matrix.scale(0.01));
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-        result.getDDRM(),
-        new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912,
-            0.03076368, 1.04111993}).getDDRM(),
-        1E-8
-    ));
-  }
-}

wpiutil/src/test/native/cpp/EigenTest.cpp

diff --git a/wpiutil/src/test/native/cpp/EigenTest.cpp b/wpiutil/src/test/native/cpp/EigenTest.cpp
deleted file mode 100644
index 04a0bc2..0000000
--- a/wpiutil/src/test/native/cpp/EigenTest.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <Eigen/Core>
-#include <Eigen/LU>
-
-#include "gtest/gtest.h"
-
-TEST(EigenTest, MultiplicationTest) {
-  Eigen::Matrix<double, 2, 2> m1;
-  m1 << 2, 1, 0, 1;
-
-  Eigen::Matrix<double, 2, 2> m2;
-  m2 << 3, 0, 0, 2.5;
-
-  const auto result = m1 * m2;
-
-  Eigen::Matrix<double, 2, 2> expectedResult;
-  expectedResult << 6.0, 2.5, 0.0, 2.5;
-
-  EXPECT_TRUE(expectedResult.isApprox(result));
-
-  Eigen::Matrix<double, 2, 3> m3;
-  m3 << 1.0, 3.0, 0.5, 2.0, 4.3, 1.2;
-
-  Eigen::Matrix<double, 3, 4> m4;
-  m4 << 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0;
-
-  const auto result2 = m3 * m4;
-
-  Eigen::Matrix<double, 2, 4> expectedResult2;
-  expectedResult2 << 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53;
-
-  EXPECT_TRUE(expectedResult2.isApprox(result2));
-}
-
-TEST(EigenTest, TransposeTest) {
-  Eigen::Matrix<double, 3, 1> vec;
-  vec << 1, 2, 3;
-
-  const auto transpose = vec.transpose();
-
-  Eigen::Matrix<double, 1, 3> expectedTranspose;
-  expectedTranspose << 1, 2, 3;
-
-  EXPECT_TRUE(expectedTranspose.isApprox(transpose));
-}
-
-TEST(EigenTest, InverseTest) {
-  Eigen::Matrix<double, 3, 3> mat;
-  mat << 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5;
-
-  const auto inverse = mat.inverse();
-  const auto identity = Eigen::MatrixXd::Identity(3, 3);
-
-  EXPECT_TRUE(identity.isApprox(mat * inverse));
-}

wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp

diff --git a/wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp b/wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp
new file mode 100644
index 0000000..8022cae
--- /dev/null
+++ b/wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "wpi/HttpWebSocketServerConnection.h"  // NOLINT(build/include_order)
+
+#include <gtest/gtest.h>
+
+namespace wpi {
+
+class HttpWebSocketServerConnectionTest
+    : public HttpWebSocketServerConnection<HttpWebSocketServerConnectionTest> {
+ public:
+  HttpWebSocketServerConnectionTest(std::shared_ptr<uv::Stream> stream,
+                                    ArrayRef<StringRef> protocols)
+      : HttpWebSocketServerConnection{stream, protocols} {}
+
+  void ProcessRequest() override { ++gotRequest; }
+  void ProcessWsUpgrade() override { ++gotUpgrade; }
+
+  int gotRequest = 0;
+  int gotUpgrade = 0;
+};
+
+}  // namespace wpi

wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp

diff --git a/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp b/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp
new file mode 100644
index 0000000..72da38b
--- /dev/null
+++ b/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp
@@ -0,0 +1,151 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "wpi/static_circular_buffer.h"  // NOLINT(build/include_order)
+
+#include <array>
+
+#include "gtest/gtest.h"
+
+static const std::array<double, 10> values = {
+    {751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727,
+     421.125, 799.913}};
+
+static const std::array<double, 8> pushFrontOut = {
+    {799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657}};
+
+static const std::array<double, 8> pushBackOut = {
+    {342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}};
+
+TEST(StaticCircularBufferTest, PushFrontTest) {
+  wpi::static_circular_buffer<double, 8> queue;
+
+  for (auto& value : values) {
+    queue.push_front(value);
+  }
+
+  for (size_t i = 0; i < pushFrontOut.size(); ++i) {
+    EXPECT_EQ(pushFrontOut[i], queue[i]);
+  }
+}
+
+TEST(StaticCircularBufferTest, PushBackTest) {
+  wpi::static_circular_buffer<double, 8> queue;
+
+  for (auto& value : values) {
+    queue.push_back(value);
+  }
+
+  for (size_t i = 0; i < pushBackOut.size(); ++i) {
+    EXPECT_EQ(pushBackOut[i], queue[i]);
+  }
+}
+
+TEST(StaticCircularBufferTest, EmplaceFrontTest) {
+  wpi::static_circular_buffer<double, 8> queue;
+
+  for (auto& value : values) {
+    queue.emplace_front(value);
+  }
+
+  for (size_t i = 0; i < pushFrontOut.size(); ++i) {
+    EXPECT_EQ(pushFrontOut[i], queue[i]);
+  }
+}
+
+TEST(StaticCircularBufferTest, EmplaceBackTest) {
+  wpi::static_circular_buffer<double, 8> queue;
+
+  for (auto& value : values) {
+    queue.emplace_back(value);
+  }
+
+  for (size_t i = 0; i < pushBackOut.size(); ++i) {
+    EXPECT_EQ(pushBackOut[i], queue[i]);
+  }
+}
+
+TEST(StaticCircularBufferTest, PushPopTest) {
+  wpi::static_circular_buffer<double, 3> queue;
+
+  // Insert three elements into the buffer
+  queue.push_back(1.0);
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+  EXPECT_EQ(3.0, queue[2]);
+
+  /*
+   * The buffer is full now, so pushing subsequent elements will overwrite the
+   * front-most elements.
+   */
+
+  queue.push_back(4.0);  // Overwrite 1 with 4
+
+  // The buffer now contains 2, 3 and 4
+  EXPECT_EQ(2.0, queue[0]);
+  EXPECT_EQ(3.0, queue[1]);
+  EXPECT_EQ(4.0, queue[2]);
+
+  queue.push_back(5.0);  // Overwrite 2 with 5
+
+  // The buffer now contains 3, 4 and 5
+  EXPECT_EQ(3.0, queue[0]);
+  EXPECT_EQ(4.0, queue[1]);
+  EXPECT_EQ(5.0, queue[2]);
+
+  EXPECT_EQ(5.0, queue.pop_back());  // 5 is removed
+
+  // The buffer now contains 3 and 4
+  EXPECT_EQ(3.0, queue[0]);
+  EXPECT_EQ(4.0, queue[1]);
+
+  EXPECT_EQ(3.0, queue.pop_front());  // 3 is removed
+
+  // Leaving only one element with value == 4
+  EXPECT_EQ(4.0, queue[0]);
+}
+
+TEST(StaticCircularBufferTest, ResetTest) {
+  wpi::static_circular_buffer<double, 5> queue;
+
+  for (size_t i = 1; i < 6; ++i) {
+    queue.push_back(i);
+  }
+
+  queue.reset();
+
+  EXPECT_EQ(queue.size(), size_t{0});
+}
+
+TEST(StaticCircularBufferTest, IteratorTest) {
+  wpi::static_circular_buffer<double, 3> queue;
+
+  queue.push_back(1.0);
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+  queue.push_back(4.0);  // Overwrite 1 with 4
+
+  // The buffer now contains 2, 3 and 4
+  const std::array<double, 3> values = {2.0, 3.0, 4.0};
+
+  // iterator
+  int i = 0;
+  for (auto& elem : queue) {
+    EXPECT_EQ(values[i], elem);
+    ++i;
+  }
+
+  // const_iterator
+  i = 0;
+  for (const auto& elem : queue) {
+    EXPECT_EQ(values[i], elem);
+    ++i;
+  }
+}

wpiutil/src/test/native/cpp/UnitsTest.cpp

diff --git a/wpiutil/src/test/native/cpp/UnitsTest.cpp b/wpiutil/src/test/native/cpp/UnitsTest.cpp
deleted file mode 100644
index 8e0823a..0000000
--- a/wpiutil/src/test/native/cpp/UnitsTest.cpp
+++ /dev/null
@@ -1,3379 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <array>
-#include <chrono>
-#include <string>
-#include <type_traits>
-
-#include "gtest/gtest.h"
-#include "units/units.h"
-
-using namespace units;
-using namespace units::dimensionless;
-using namespace units::length;
-using namespace units::mass;
-using namespace units::angle;
-using namespace units::time;
-using namespace units::frequency;
-using namespace units::area;
-using namespace units::velocity;
-using namespace units::angular_velocity;
-using namespace units::temperature;
-using namespace units::luminous_intensity;
-using namespace units::solid_angle;
-using namespace units::frequency;
-using namespace units::acceleration;
-using namespace units::pressure;
-using namespace units::charge;
-using namespace units::energy;
-using namespace units::power;
-using namespace units::voltage;
-using namespace units::capacitance;
-using namespace units::impedance;
-using namespace units::conductance;
-using namespace units::magnetic_flux;
-using namespace units::magnetic_field_strength;
-using namespace units::inductance;
-using namespace units::luminous_flux;
-using namespace units::illuminance;
-using namespace units::radiation;
-using namespace units::torque;
-using namespace units::volume;
-using namespace units::density;
-using namespace units::concentration;
-using namespace units::data;
-using namespace units::data_transfer_rate;
-using namespace units::math;
-
-#if !defined(_MSC_VER) || _MSC_VER > 1800
-using namespace units::literals;
-#endif
-
-namespace {
-
-class TypeTraits : public ::testing::Test {
- protected:
-  TypeTraits() {}
-  virtual ~TypeTraits() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-};
-
-class UnitManipulators : public ::testing::Test {
- protected:
-  UnitManipulators() {}
-  virtual ~UnitManipulators() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-};
-
-class UnitContainer : public ::testing::Test {
- protected:
-  UnitContainer() {}
-  virtual ~UnitContainer() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-};
-
-class UnitConversion : public ::testing::Test {
- protected:
-  UnitConversion() {}
-  virtual ~UnitConversion() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-};
-
-class UnitMath : public ::testing::Test {
- protected:
-  UnitMath() {}
-  virtual ~UnitMath() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-};
-
-class CompileTimeArithmetic : public ::testing::Test {
- protected:
-  CompileTimeArithmetic() {}
-  virtual ~CompileTimeArithmetic() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-};
-
-class Constexpr : public ::testing::Test {
- protected:
-  Constexpr() {}
-  virtual ~Constexpr() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-};
-
-class CaseStudies : public ::testing::Test {
- protected:
-  CaseStudies() {}
-  virtual ~CaseStudies() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
-
-  struct RightTriangle {
-    using a = unit_value_t<meters, 3>;
-    using b = unit_value_t<meters, 4>;
-    using c = unit_value_sqrt<
-        unit_value_add<unit_value_power<a, 2>, unit_value_power<b, 2>>>;
-  };
-};
-}  // namespace
-
-TEST_F(TypeTraits, isRatio) {
-  EXPECT_TRUE(traits::is_ratio<std::ratio<1>>::value);
-  EXPECT_FALSE(traits::is_ratio<double>::value);
-}
-
-TEST_F(TypeTraits, ratio_sqrt) {
-  using rt2 = ratio_sqrt<std::ratio<2>>;
-  EXPECT_LT(std::abs(std::sqrt(2 / static_cast<double>(1)) -
-                     rt2::num / static_cast<double>(rt2::den)),
-            5e-9);
-
-  using rt4 = ratio_sqrt<std::ratio<4>>;
-  EXPECT_LT(std::abs(std::sqrt(4 / static_cast<double>(1)) -
-                     rt4::num / static_cast<double>(rt4::den)),
-            5e-9);
-
-  using rt10 = ratio_sqrt<std::ratio<10>>;
-  EXPECT_LT(std::abs(std::sqrt(10 / static_cast<double>(1)) -
-                     rt10::num / static_cast<double>(rt10::den)),
-            5e-9);
-
-  using rt30 = ratio_sqrt<std::ratio<30>>;
-  EXPECT_LT(std::abs(std::sqrt(30 / static_cast<double>(1)) -
-                     rt30::num / static_cast<double>(rt30::den)),
-            5e-9);
-
-  using rt61 = ratio_sqrt<std::ratio<61>>;
-  EXPECT_LT(std::abs(std::sqrt(61 / static_cast<double>(1)) -
-                     rt61::num / static_cast<double>(rt61::den)),
-            5e-9);
-
-  using rt100 = ratio_sqrt<std::ratio<100>>;
-  EXPECT_LT(std::abs(std::sqrt(100 / static_cast<double>(1)) -
-                     rt100::num / static_cast<double>(rt100::den)),
-            5e-9);
-
-  using rt1000 = ratio_sqrt<std::ratio<1000>>;
-  EXPECT_LT(std::abs(std::sqrt(1000 / static_cast<double>(1)) -
-                     rt1000::num / static_cast<double>(rt1000::den)),
-            5e-9);
-
-  using rt10000 = ratio_sqrt<std::ratio<10000>>;
-  EXPECT_LT(std::abs(std::sqrt(10000 / static_cast<double>(1)) -
-                     rt10000::num / static_cast<double>(rt10000::den)),
-            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_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);
-}
-
-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));
-}
-
-TEST_F(TypeTraits, unit_t_traits) {
-  EXPECT_TRUE(
-      (std::is_same<void,
-                    traits::unit_t_traits<double>::underlying_type>::value));
-  EXPECT_TRUE(
-      (std::is_same<UNIT_LIB_DEFAULT_TYPE,
-                    traits::unit_t_traits<meter_t>::underlying_type>::value));
-  EXPECT_TRUE(
-      (std::is_same<void, traits::unit_t_traits<double>::value_type>::value));
-  EXPECT_TRUE(
-      (std::is_same<UNIT_LIB_DEFAULT_TYPE,
-                    traits::unit_t_traits<meter_t>::value_type>::value));
-}
-
-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));
-}
-
-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<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_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));
-}
-
-TEST_F(TypeTraits, inverse) {
-  double test;
-
-  using htz = inverse<seconds>;
-  bool shouldBeTrue = std::is_same<htz, hertz>::value;
-  EXPECT_TRUE(shouldBeTrue);
-
-  test = convert<inverse<celsius>, inverse<fahrenheit>>(1.0);
-  EXPECT_NEAR(5.0 / 9.0, test, 5.0e-5);
-
-  test = convert<inverse<kelvin>, inverse<fahrenheit>>(6.0);
-  EXPECT_NEAR(10.0 / 3.0, test, 5.0e-5);
-}
-
-TEST_F(TypeTraits, base_unit_of) {
-  using base = traits::base_unit_of<years>;
-  bool shouldBeTrue = std::is_same<base, category::time_unit>::value;
-
-  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));
-}
-
-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_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));
-}
-
-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));
-}
-
-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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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<substance::mole_t, substance::mole_t>::value));
-  EXPECT_FALSE((traits::is_substance_unit<year_t, substance::mole_t>::value));
-}
-
-TEST_F(TypeTraits, is_luminous_intensity_unit) {
-  EXPECT_TRUE((traits::is_luminous_intensity_unit<candela>::value));
-  EXPECT_FALSE(
-      (traits::is_luminous_intensity_unit<units::radiation::rad>::value));
-  EXPECT_FALSE((traits::is_luminous_intensity_unit<double>::value));
-
-  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<candela_t, candela_t>::value));
-  EXPECT_FALSE((traits::is_luminous_intensity_unit<rad_t, candela_t>::value));
-}
-
-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<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<degree_squared_t, steradian_t>::value));
-  EXPECT_FALSE(
-      (traits::is_solid_angle_unit<angle::degree_t, steradian_t>::value));
-}
-
-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<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));
-}
-
-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<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<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));
-}
-
-TEST_F(TypeTraits, is_acceleration_unit) {
-  EXPECT_TRUE((traits::is_acceleration_unit<meters_per_second_squared>::value));
-  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));
-
-  EXPECT_TRUE(
-      (traits::is_acceleration_unit<meters_per_second_squared_t>::value));
-  EXPECT_TRUE(
-      (traits::is_acceleration_unit<const meters_per_second_squared_t>::value));
-  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));
-  EXPECT_TRUE(
-      (traits::is_acceleration_unit<standard_gravity_t,
-                                    meters_per_second_squared_t>::value));
-  EXPECT_FALSE(
-      (traits::is_acceleration_unit<inch_t,
-                                    meters_per_second_squared_t>::value));
-}
-
-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<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));
-}
-
-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<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<atmosphere_t, pressure::pascal_t>::value));
-  EXPECT_FALSE((traits::is_pressure_unit<year_t, pressure::pascal_t>::value));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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<const farad_t&, millifarad_t>::value));
-  EXPECT_FALSE((traits::is_capacitance_unit<ohm_t, farad_t>::value));
-}
-
-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<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));
-}
-
-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<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<const siemens_t&, millisiemens_t>::value));
-  EXPECT_FALSE((traits::is_conductance_unit<volt_t, siemens_t>::value));
-}
-
-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<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));
-}
-
-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<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<gauss_t, tesla_t>::value));
-  EXPECT_FALSE(
-      (traits::is_magnetic_field_strength_unit<volt_t, tesla_t>::value));
-}
-
-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<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<const henry_t&, millihenry_t>::value));
-  EXPECT_FALSE((traits::is_inductance_unit<farad_t, henry_t>::value));
-}
-
-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<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<const lumen_t&, millilumen_t>::value));
-  EXPECT_FALSE((traits::is_luminous_flux_unit<pound_t, lumen_t>::value));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<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));
-}
-
-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<kilograms_per_cubic_meter_t>::value));
-  EXPECT_TRUE(
-      (traits::is_density_unit<const kilograms_per_cubic_meter_t>::value));
-  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));
-  EXPECT_FALSE(
-      (traits::is_density_unit<year_t, kilograms_per_cubic_meter_t>::value));
-}
-
-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<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));
-}
-
-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<gigabits_per_second_t>::value));
-  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));
-  EXPECT_TRUE(
-      (traits::is_data_transfer_rate_unit<gigabytes_per_second_t>::value));
-  EXPECT_FALSE((traits::is_data_transfer_rate_unit<year_t>::value));
-  EXPECT_TRUE(
-      (traits::is_data_transfer_rate_unit<gigabits_per_second_t,
-                                          gigabytes_per_second_t>::value));
-  EXPECT_FALSE(
-      (traits::is_data_transfer_rate_unit<year_t,
-                                          gigabytes_per_second_t>::value));
-}
-
-TEST_F(UnitManipulators, squared) {
-  double test;
-
-  test = convert<squared<meters>, square_feet>(0.092903);
-  EXPECT_NEAR(0.99999956944, test, 5.0e-12);
-
-  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;
-  EXPECT_TRUE(isSame);
-}
-
-TEST_F(UnitManipulators, cubed) {
-  double test;
-
-  test = convert<cubed<meters>, cubic_feet>(0.0283168);
-  EXPECT_NEAR(0.999998354619, test, 5.0e-13);
-}
-
-TEST_F(UnitManipulators, square_root) {
-  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_NEAR(1000.0, test, 5.0e-13);
-}
-
-TEST_F(UnitManipulators, compound_unit) {
-  using acceleration1 = unit<std::ratio<1>, category::acceleration_unit>;
-  using acceleration2 =
-      compound_unit<meters, inverse<seconds>, inverse<seconds>>;
-  using acceleration3 =
-      unit<std::ratio<1>,
-           base_unit<std::ratio<1>, std::ratio<0>, std::ratio<-2>>>;
-  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;
-
-  EXPECT_TRUE(areSame12);
-  EXPECT_TRUE(areSame23);
-  EXPECT_TRUE(areSame34);
-  EXPECT_TRUE(areSame45);
-
-  // test that thing with translations still compile
-  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));
-}
-
-TEST_F(UnitManipulators, dimensionalAnalysis) {
-  // these look like 'compound units', but the dimensional analysis can be
-  // REALLY handy if the unit types aren't know (i.e. they themselves are
-  // 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;
-  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;
-  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<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));
-}
-#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));
-}
-
-TEST_F(UnitContainer, make_unit) {
-  auto dist = units::make_unit<meter_t>(5);
-  EXPECT_EQ(meter_t(5), dist);
-}
-
-TEST_F(UnitContainer, unitTypeAddition) {
-  // units
-  meter_t a_m(1.0), c_m;
-  foot_t b_ft(3.28084);
-
-  double d = convert<feet, meters>(b_ft());
-  EXPECT_NEAR(1.0, d, 5.0e-5);
-
-  c_m = a_m + b_ft;
-  EXPECT_NEAR(2.0, c_m(), 5.0e-5);
-
-  c_m = b_ft + meter_t(3);
-  EXPECT_NEAR(4.0, c_m(), 5.0e-5);
-
-  auto e_ft = b_ft + meter_t(3);
-  EXPECT_NEAR(13.12336, e_ft(), 5.0e-6);
-
-  // scalar
-  scalar_t sresult = scalar_t(1.0) + scalar_t(1.0);
-  EXPECT_NEAR(2.0, sresult, 5.0e-6);
-
-  sresult = scalar_t(1.0) + 1.0;
-  EXPECT_NEAR(2.0, sresult, 5.0e-6);
-
-  sresult = 1.0 + scalar_t(1.0);
-  EXPECT_NEAR(2.0, sresult, 5.0e-6);
-
-  d = scalar_t(1.0) + scalar_t(1.0);
-  EXPECT_NEAR(2.0, d, 5.0e-6);
-
-  d = scalar_t(1.0) + 1.0;
-  EXPECT_NEAR(2.0, d, 5.0e-6);
-
-  d = 1.0 + scalar_t(1.0);
-  EXPECT_NEAR(2.0, d, 5.0e-6);
-}
-
-TEST_F(UnitContainer, unitTypeUnaryAddition) {
-  meter_t a_m(1.0);
-
-  EXPECT_EQ(++a_m, meter_t(2));
-  EXPECT_EQ(a_m++, meter_t(2));
-  EXPECT_EQ(a_m, meter_t(3));
-  EXPECT_EQ(+a_m, meter_t(3));
-  EXPECT_EQ(a_m, meter_t(3));
-
-  dBW_t b_dBW(1.0);
-
-  EXPECT_EQ(++b_dBW, dBW_t(2));
-  EXPECT_EQ(b_dBW++, dBW_t(2));
-  EXPECT_EQ(b_dBW, dBW_t(3));
-  EXPECT_EQ(+b_dBW, dBW_t(3));
-  EXPECT_EQ(b_dBW, dBW_t(3));
-}
-
-TEST_F(UnitContainer, unitTypeSubtraction) {
-  meter_t a_m(1.0), c_m;
-  foot_t b_ft(3.28084);
-
-  c_m = a_m - b_ft;
-  EXPECT_NEAR(0.0, c_m(), 5.0e-5);
-
-  c_m = b_ft - meter_t(1);
-  EXPECT_NEAR(0.0, c_m(), 5.0e-5);
-
-  auto e_ft = b_ft - meter_t(1);
-  EXPECT_NEAR(0.0, e_ft(), 5.0e-6);
-
-  scalar_t sresult = scalar_t(1.0) - scalar_t(1.0);
-  EXPECT_NEAR(0.0, sresult, 5.0e-6);
-
-  sresult = scalar_t(1.0) - 1.0;
-  EXPECT_NEAR(0.0, sresult, 5.0e-6);
-
-  sresult = 1.0 - scalar_t(1.0);
-  EXPECT_NEAR(0.0, sresult, 5.0e-6);
-
-  double d = scalar_t(1.0) - scalar_t(1.0);
-  EXPECT_NEAR(0.0, d, 5.0e-6);
-
-  d = scalar_t(1.0) - 1.0;
-  EXPECT_NEAR(0.0, d, 5.0e-6);
-
-  d = 1.0 - scalar_t(1.0);
-  EXPECT_NEAR(0.0, d, 5.0e-6);
-}
-
-TEST_F(UnitContainer, unitTypeUnarySubtraction) {
-  meter_t a_m(4.0);
-
-  EXPECT_EQ(--a_m, meter_t(3));
-  EXPECT_EQ(a_m--, meter_t(3));
-  EXPECT_EQ(a_m, meter_t(2));
-  EXPECT_EQ(-a_m, meter_t(-2));
-  EXPECT_EQ(a_m, meter_t(2));
-
-  dBW_t b_dBW(4.0);
-
-  EXPECT_EQ(--b_dBW, dBW_t(3));
-  EXPECT_EQ(b_dBW--, dBW_t(3));
-  EXPECT_EQ(b_dBW, dBW_t(2));
-  EXPECT_EQ(-b_dBW, dBW_t(-2));
-  EXPECT_EQ(b_dBW, dBW_t(2));
-}
-
-TEST_F(UnitContainer, unitTypeMultiplication) {
-  meter_t a_m(1.0), b_m(2.0);
-  foot_t a_ft(3.28084);
-
-  auto c_m2 = a_m * b_m;
-  EXPECT_NEAR(2.0, c_m2(), 5.0e-5);
-
-  c_m2 = b_m * meter_t(2);
-  EXPECT_NEAR(4.0, c_m2(), 5.0e-5);
-
-  c_m2 = b_m * a_ft;
-  EXPECT_NEAR(2.0, c_m2(), 5.0e-5);
-
-  auto c_m = b_m * 2.0;
-  EXPECT_NEAR(4.0, c_m(), 5.0e-5);
-
-  c_m = 2.0 * b_m;
-  EXPECT_NEAR(4.0, c_m(), 5.0e-5);
-
-  double convert = scalar_t(3.14);
-  EXPECT_NEAR(3.14, convert, 5.0e-5);
-
-  scalar_t sresult = scalar_t(5.0) * scalar_t(4.0);
-  EXPECT_NEAR(20.0, sresult(), 5.0e-5);
-
-  sresult = scalar_t(5.0) * 4.0;
-  EXPECT_NEAR(20.0, sresult(), 5.0e-5);
-
-  sresult = 4.0 * scalar_t(5.0);
-  EXPECT_NEAR(20.0, sresult(), 5.0e-5);
-
-  double result = scalar_t(5.0) * scalar_t(4.0);
-  EXPECT_NEAR(20.0, result, 5.0e-5);
-
-  result = scalar_t(5.0) * 4.0;
-  EXPECT_NEAR(20.0, result, 5.0e-5);
-
-  result = 4.0 * scalar_t(5.0);
-  EXPECT_NEAR(20.0, result, 5.0e-5);
-}
-
-TEST_F(UnitContainer, unitTypeMixedUnitMultiplication) {
-  meter_t a_m(1.0);
-  foot_t b_ft(3.28084);
-  unit_t<inverse<meter>> i_m(2.0);
-
-  // resultant unit is square of leftmost unit
-  auto c_m2 = a_m * b_ft;
-  EXPECT_NEAR(1.0, c_m2(), 5.0e-5);
-
-  auto c_ft2 = b_ft * a_m;
-  EXPECT_NEAR(10.7639111056, c_ft2(), 5.0e-7);
-
-  // you can get whatever (compatible) type you want if you ask explicitly
-  square_meter_t d_m2 = b_ft * a_m;
-  EXPECT_NEAR(1.0, d_m2(), 5.0e-5);
-
-  // a unit times a sclar ends up with the same units.
-  meter_t e_m = a_m * scalar_t(3.0);
-  EXPECT_NEAR(3.0, e_m(), 5.0e-5);
-
-  e_m = scalar_t(4.0) * a_m;
-  EXPECT_NEAR(4.0, e_m(), 5.0e-5);
-
-  // unit times its inverse results in a scalar
-  scalar_t s = a_m * i_m;
-  EXPECT_NEAR(2.0, s, 5.0e-5);
-
-  c_m2 = b_ft * meter_t(2);
-  EXPECT_NEAR(2.0, c_m2(), 5.0e-5);
-
-  auto e_ft2 = b_ft * meter_t(3);
-  EXPECT_NEAR(32.2917333168, e_ft2(), 5.0e-6);
-
-  auto mps = meter_t(10.0) * unit_t<inverse<seconds>>(1.0);
-  EXPECT_EQ(mps, meters_per_second_t(10));
-}
-
-TEST_F(UnitContainer, unitTypeScalarMultiplication) {
-  meter_t a_m(1.0);
-
-  auto result_m = scalar_t(3.0) * a_m;
-  EXPECT_NEAR(3.0, result_m(), 5.0e-5);
-
-  result_m = a_m * scalar_t(4.0);
-  EXPECT_NEAR(4.0, result_m(), 5.0e-5);
-
-  result_m = 3.0 * a_m;
-  EXPECT_NEAR(3.0, result_m(), 5.0e-5);
-
-  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;
-  EXPECT_TRUE(isSame);
-}
-
-TEST_F(UnitContainer, unitTypeDivision) {
-  meter_t a_m(1.0), b_m(2.0);
-  foot_t a_ft(3.28084);
-  second_t a_sec(10.0);
-  bool isSame;
-
-  auto c = a_m / a_ft;
-  EXPECT_NEAR(1.0, c, 5.0e-5);
-  isSame = std::is_same<decltype(c), scalar_t>::value;
-  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;
-  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;
-  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;
-  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;
-  EXPECT_TRUE(isSame);
-
-  double d = scalar_t(1.0) / 2.0;
-  EXPECT_NEAR(0.5, d, 5.0e-5);
-
-  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;
-  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;
-  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;
-  EXPECT_TRUE(isSame);
-
-  auto mph = mile_t(60.0) / hour_t(1.0);
-  meters_per_second_t mps = mph;
-  EXPECT_NEAR(26.8224, mps(), 5.0e-5);
-}
-
-TEST_F(UnitContainer, compoundAssignmentAddition) {
-  // units
-  meter_t a(0.0);
-  a += meter_t(1.0);
-
-  EXPECT_EQ(meter_t(1.0), a);
-
-  a += foot_t(meter_t(1));
-
-  EXPECT_EQ(meter_t(2.0), a);
-
-  // scalars
-  scalar_t b(0);
-  b += scalar_t(1.0);
-
-  EXPECT_EQ(scalar_t(1.0), b);
-
-  b += 1;
-
-  EXPECT_EQ(scalar_t(2.0), b);
-}
-
-TEST_F(UnitContainer, compoundAssignmentSubtraction) {
-  // units
-  meter_t a(2.0);
-  a -= meter_t(1.0);
-
-  EXPECT_EQ(meter_t(1.0), a);
-
-  a -= foot_t(meter_t(1));
-
-  EXPECT_EQ(meter_t(0.0), a);
-
-  // scalars
-  scalar_t b(2);
-  b -= scalar_t(1.0);
-
-  EXPECT_EQ(scalar_t(1.0), b);
-
-  b -= 1;
-
-  EXPECT_EQ(scalar_t(0), b);
-}
-
-TEST_F(UnitContainer, compoundAssignmentMultiplication) {
-  // units
-  meter_t a(2.0);
-  a *= scalar_t(2.0);
-
-  EXPECT_EQ(meter_t(4.0), a);
-
-  a *= 2.0;
-
-  EXPECT_EQ(meter_t(8.0), a);
-
-  // scalars
-  scalar_t b(2);
-  b *= scalar_t(2.0);
-
-  EXPECT_EQ(scalar_t(4.0), b);
-
-  b *= 2;
-
-  EXPECT_EQ(scalar_t(8.0), b);
-}
-
-TEST_F(UnitContainer, compoundAssignmentDivision) {
-  // units
-  meter_t a(8.0);
-  a /= scalar_t(2.0);
-
-  EXPECT_EQ(meter_t(4.0), a);
-
-  a /= 2.0;
-
-  EXPECT_EQ(meter_t(2.0), a);
-
-  // scalars
-  scalar_t b(8);
-  b /= scalar_t(2.0);
-
-  EXPECT_EQ(scalar_t(4.0), b);
-
-  b /= 2;
-
-  EXPECT_EQ(scalar_t(2.0), b);
-}
-
-TEST_F(UnitContainer, scalarTypeImplicitConversion) {
-  double test = scalar_t(3.0);
-  EXPECT_DOUBLE_EQ(3.0, test);
-
-  scalar_t testS = 3.0;
-  EXPECT_DOUBLE_EQ(3.0, testS);
-
-  scalar_t test3(ppm_t(10));
-  EXPECT_DOUBLE_EQ(0.00001, test3);
-
-  scalar_t test4;
-  test4 = ppm_t(1);
-  EXPECT_DOUBLE_EQ(0.000001, test4);
-}
-
-TEST_F(UnitContainer, valueMethod) {
-  double test = meter_t(3.0).to<double>();
-  EXPECT_DOUBLE_EQ(3.0, test);
-
-  auto test2 = meter_t(4.0).value();
-  EXPECT_DOUBLE_EQ(4.0, test2);
-  EXPECT_TRUE((std::is_same<decltype(test2), double>::value));
-}
-
-TEST_F(UnitContainer, convertMethod) {
-  double test = meter_t(3.0).convert<feet>().to<double>();
-  EXPECT_NEAR(9.84252, test, 5.0e-6);
-}
-
-#ifndef UNIT_LIB_DISABLE_IOSTREAM
-TEST_F(UnitContainer, cout) {
-  testing::internal::CaptureStdout();
-  std::cout << degree_t(349.87);
-  std::string output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("349.87 deg", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << meter_t(1.0);
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("1 m", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << dB_t(31.0);
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("31 dB", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << volt_t(21.79);
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("21.79 V", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << dBW_t(12.0);
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("12 dBW", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << dBm_t(120.0);
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("120 dBm", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << miles_per_hour_t(72.1);
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("72.1 mph", output.c_str());
-
-  // undefined unit
-  testing::internal::CaptureStdout();
-  std::cout << units::math::cpow<4>(meter_t(2));
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("16 m^4", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << units::math::cpow<3>(foot_t(2));
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("8 cu_ft", output.c_str());
-
-  testing::internal::CaptureStdout();
-  std::cout << std::setprecision(9) << units::math::cpow<4>(foot_t(2));
-  output = testing::internal::GetCapturedStdout();
-  EXPECT_STREQ("0.138095597 m^4", output.c_str());
-
-  // constants
-  testing::internal::CaptureStdout();
-  std::cout << std::setprecision(8) << constants::k_B;
-  output = testing::internal::GetCapturedStdout();
-#if defined(_MSC_VER) && (_MSC_VER <= 1800)
-  EXPECT_STREQ("1.3806485e-023 m^2 kg s^-2 K^-1", output.c_str());
-#else
-  EXPECT_STREQ("1.3806485e-23 m^2 kg s^-2 K^-1", output.c_str());
-#endif
-
-  testing::internal::CaptureStdout();
-  std::cout << std::setprecision(9) << constants::mu_B;
-  output = testing::internal::GetCapturedStdout();
-#if defined(_MSC_VER) && (_MSC_VER <= 1800)
-  EXPECT_STREQ("9.27400999e-024 m^2 A", output.c_str());
-#else
-  EXPECT_STREQ("9.27400999e-24 m^2 A", output.c_str());
-#endif
-
-  testing::internal::CaptureStdout();
-  std::cout << std::setprecision(7) << constants::sigma;
-  output = testing::internal::GetCapturedStdout();
-#if defined(_MSC_VER) && (_MSC_VER <= 1800)
-  EXPECT_STREQ("5.670367e-008 kg s^-3 K^-4", output.c_str());
-#else
-  EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
-#endif
-}
-
-TEST_F(UnitContainer, to_string) {
-  foot_t a(3.5);
-  EXPECT_STREQ("3.5 ft", units::length::to_string(a).c_str());
-
-  meter_t b(8);
-  EXPECT_STREQ("8 m", units::length::to_string(b).c_str());
-}
-
-TEST_F(UnitContainer, DISABLED_to_string_locale) {
-  struct lconv* lc;
-
-  // German locale
-#if defined(_MSC_VER)
-  setlocale(LC_ALL, "de-DE");
-#else
-  EXPECT_STREQ("de_DE.utf8", setlocale(LC_ALL, "de_DE.utf8"));
-#endif
-
-  lc = localeconv();
-  char point_de = *lc->decimal_point;
-  EXPECT_EQ(point_de, ',');
-
-  kilometer_t de = 2_km;
-  EXPECT_STREQ("2 km", units::length::to_string(de).c_str());
-
-  de = 2.5_km;
-  EXPECT_STREQ("2,5 km", units::length::to_string(de).c_str());
-
-  // US locale
-#if defined(_MSC_VER)
-  setlocale(LC_ALL, "en-US");
-#else
-  EXPECT_STREQ("en_US.utf8", setlocale(LC_ALL, "en_US.utf8"));
-#endif
-
-  lc = localeconv();
-  char point_us = *lc->decimal_point;
-  EXPECT_EQ(point_us, '.');
-
-  mile_t us = 2_mi;
-  EXPECT_STREQ("2 mi", units::length::to_string(us).c_str());
-
-  us = 2.5_mi;
-  EXPECT_STREQ("2.5 mi", units::length::to_string(us).c_str());
-}
-
-TEST_F(UnitContainer, nameAndAbbreviation) {
-  foot_t a(3.5);
-  EXPECT_STREQ("ft", units::abbreviation(a));
-  EXPECT_STREQ("ft", a.abbreviation());
-  EXPECT_STREQ("foot", a.name());
-
-  meter_t b(8);
-  EXPECT_STREQ("m", units::abbreviation(b));
-  EXPECT_STREQ("m", b.abbreviation());
-  EXPECT_STREQ("meter", b.name());
-}
-#endif
-
-TEST_F(UnitContainer, negative) {
-  meter_t a(5.3);
-  meter_t b(-5.3);
-  EXPECT_NEAR(a.to<double>(), -b.to<double>(), 5.0e-320);
-  EXPECT_NEAR(b.to<double>(), -a.to<double>(), 5.0e-320);
-
-  dB_t c(2.87);
-  dB_t d(-2.87);
-  EXPECT_NEAR(c.to<double>(), -d.to<double>(), 5.0e-320);
-  EXPECT_NEAR(d.to<double>(), -c.to<double>(), 5.0e-320);
-
-  ppm_t e = -1 * ppm_t(10);
-  EXPECT_EQ(e, -ppm_t(10));
-  EXPECT_NEAR(-0.00001, e, 5.0e-10);
-}
-
-TEST_F(UnitContainer, concentration) {
-  ppb_t a(ppm_t(1));
-  EXPECT_EQ(ppb_t(1000), a);
-  EXPECT_EQ(0.000001, a);
-  EXPECT_EQ(0.000001, a.to<double>());
-
-  scalar_t b(ppm_t(1));
-  EXPECT_EQ(0.000001, b);
-
-  scalar_t c = ppb_t(1);
-  EXPECT_EQ(0.000000001, c);
-}
-
-TEST_F(UnitContainer, dBConversion) {
-  dBW_t a_dbw(23.1);
-  watt_t a_w = a_dbw;
-  dBm_t a_dbm = a_dbw;
-
-  EXPECT_NEAR(204.173794, a_w(), 5.0e-7);
-  EXPECT_NEAR(53.1, a_dbm(), 5.0e-7);
-
-  milliwatt_t b_mw(100000.0);
-  watt_t b_w = b_mw;
-  dBm_t b_dbm = b_mw;
-  dBW_t b_dbw = b_mw;
-
-  EXPECT_NEAR(100.0, b_w(), 5.0e-7);
-  EXPECT_NEAR(50.0, b_dbm(), 5.0e-7);
-  EXPECT_NEAR(20.0, b_dbw(), 5.0e-7);
-}
-
-TEST_F(UnitContainer, dBAddition) {
-  bool isSame;
-
-  auto result_dbw = dBW_t(10.0) + dB_t(30.0);
-  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;
-  EXPECT_TRUE(isSame);
-
-  auto result_dbm = dB_t(30.0) + dBm_t(20.0);
-  EXPECT_NEAR(50.0, result_dbm(), 5.0e-5);
-
-  // adding dBW to dBW is something you probably shouldn't do, but let's see if
-  // 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;
-  EXPECT_TRUE(isSame);
-}
-
-TEST_F(UnitContainer, dBSubtraction) {
-  bool isSame;
-
-  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;
-  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;
-  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;
-  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;
-  EXPECT_TRUE(isSame);
-}
-
-TEST_F(UnitContainer, unit_cast) {
-  meter_t test1(5.7);
-  hectare_t test2(16);
-
-  double dResult1 = 5.7;
-
-  double dResult2 = 16;
-  int iResult2 = 16;
-
-  EXPECT_EQ(dResult1, unit_cast<double>(test1));
-  EXPECT_EQ(dResult2, unit_cast<double>(test2));
-  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));
-}
-
-// 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(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(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(meter_t(10) == x);
-
-  // conversion using literal syntax
-  foot_t y = 0.3048_m;
-  EXPECT_TRUE(1_ft == y);
-
-  // Pythagorean theorem
-  meter_t a = 3_m;
-  meter_t b = 4_m;
-  meter_t c = sqrt(pow<2>(a) + pow<2>(b));
-  EXPECT_TRUE(c == 5_m);
-}
-#endif
-
-TEST_F(UnitConversion, length) {
-  double test;
-  test = convert<meters, nanometers>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, micrometers>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, millimeters>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, centimeters>(0.01);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, kilometers>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, meters>(1.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, feet>(0.3048);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, miles>(1609.344);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, inches>(0.0254);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, nauticalMiles>(1852.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, astronicalUnits>(149597870700.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, lightyears>(9460730472580800.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<meters, parsec>(3.08567758e16);
-  EXPECT_NEAR(1.0, test, 5.0e7);
-
-  test = convert<feet, feet>(6.3);
-  EXPECT_NEAR(6.3, test, 5.0e-5);
-  test = convert<feet, inches>(6.0);
-  EXPECT_NEAR(72.0, test, 5.0e-5);
-  test = convert<inches, feet>(6.0);
-  EXPECT_NEAR(0.5, test, 5.0e-5);
-  test = convert<meter, feet>(1.0);
-  EXPECT_NEAR(3.28084, test, 5.0e-5);
-  test = convert<miles, nauticalMiles>(6.3);
-  EXPECT_NEAR(5.47455, test, 5.0e-6);
-  test = convert<miles, meters>(11.0);
-  EXPECT_NEAR(17702.8, test, 5.0e-2);
-  test = convert<meters, chains>(1.0);
-  EXPECT_NEAR(0.0497097, test, 5.0e-7);
-}
-
-TEST_F(UnitConversion, mass) {
-  double test;
-
-  test = convert<kilograms, grams>(1.0e-3);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, micrograms>(1.0e-9);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, milligrams>(1.0e-6);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, kilograms>(1.0);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, metric_tons>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, pounds>(0.453592);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, long_tons>(1016.05);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, short_tons>(907.185);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, mass::ounces>(0.0283495);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<kilograms, carats>(0.0002);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<slugs, kilograms>(1.0);
-  EXPECT_NEAR(14.593903, test, 5.0e-7);
-
-  test = convert<pounds, carats>(6.3);
-  EXPECT_NEAR(14288.2, test, 5.0e-2);
-}
-
-TEST_F(UnitConversion, time) {
-  double result = 0;
-  double daysPerYear = 365;
-  double hoursPerDay = 24;
-  double minsPerHour = 60;
-  double secsPerMin = 60;
-  double daysPerWeek = 7;
-
-  result = 2 * daysPerYear * hoursPerDay * minsPerHour * secsPerMin *
-           (1 / minsPerHour) * (1 / secsPerMin) * (1 / hoursPerDay) *
-           (1 / daysPerWeek);
-  EXPECT_NEAR(104.286, result, 5.0e-4);
-
-  year_t twoYears(2.0);
-  week_t twoYearsInWeeks = twoYears;
-  EXPECT_NEAR(week_t(104.286).to<double>(), twoYearsInWeeks.to<double>(),
-              5.0e-4);
-
-  double test;
-
-  test = convert<seconds, seconds>(1.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, nanoseconds>(1.0e-9);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, microseconds>(1.0e-6);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, milliseconds>(1.0e-3);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, minutes>(60.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, hours>(3600.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, days>(86400.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, weeks>(604800.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<seconds, years>(3.154e7);
-  EXPECT_NEAR(1.0, test, 5.0e3);
-
-  test = convert<years, weeks>(2.0);
-  EXPECT_NEAR(104.2857142857143, test, 5.0e-14);
-  test = convert<hours, minutes>(4.0);
-  EXPECT_NEAR(240.0, test, 5.0e-14);
-  test = convert<julian_years, days>(1.0);
-  EXPECT_NEAR(365.25, test, 5.0e-14);
-  test = convert<gregorian_years, days>(1.0);
-  EXPECT_NEAR(365.2425, test, 5.0e-14);
-}
-
-TEST_F(UnitConversion, angle) {
-  angle::degree_t quarterCircleDeg(90.0);
-  angle::radian_t quarterCircleRad = quarterCircleDeg;
-  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).to<double>(),
-              quarterCircleRad.to<double>(), 5.0e-12);
-
-  double test;
-
-  test = convert<angle::radians, angle::radians>(1.0);
-  EXPECT_NEAR(1.0, test, 5.0e-20);
-  test = convert<angle::radians, angle::milliradians>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-4);
-  test = convert<angle::radians, angle::degrees>(0.0174533);
-  EXPECT_NEAR(1.0, test, 5.0e-7);
-  test = convert<angle::radians, angle::arcminutes>(0.000290888);
-  EXPECT_NEAR(0.99999928265913, test, 5.0e-8);
-  test = convert<angle::radians, angle::arcseconds>(4.8481e-6);
-  EXPECT_NEAR(0.999992407, test, 5.0e-10);
-  test = convert<angle::radians, angle::turns>(6.28319);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-  test = convert<angle::radians, angle::gradians>(0.015708);
-  EXPECT_NEAR(1.0, test, 5.0e-6);
-
-  test = convert<angle::radians, angle::radians>(2.1);
-  EXPECT_NEAR(2.1, test, 5.0e-6);
-  test = convert<angle::arcseconds, angle::gradians>(2.1);
-  EXPECT_NEAR(0.000648148, test, 5.0e-6);
-  test = convert<angle::radians, angle::degrees>(constants::detail::PI_VAL);
-  EXPECT_NEAR(180.0, test, 5.0e-6);
-  test = convert<angle::degrees, angle::radians>(90.0);
-  EXPECT_NEAR(constants::detail::PI_VAL / 2, test, 5.0e-6);
-}
-
-TEST_F(UnitConversion, current) {
-  double test;
-
-  test = convert<current::A, current::mA>(2.1);
-  EXPECT_NEAR(2100.0, test, 5.0e-6);
-}
-
-TEST_F(UnitConversion, temperature) {
-  // temp conversion are weird/hard since they involve translations AND scaling.
-  double test;
-
-  test = convert<kelvin, kelvin>(72.0);
-  EXPECT_NEAR(72.0, test, 5.0e-5);
-  test = convert<fahrenheit, fahrenheit>(72.0);
-  EXPECT_NEAR(72.0, test, 5.0e-5);
-  test = convert<kelvin, fahrenheit>(300.0);
-  EXPECT_NEAR(80.33, test, 5.0e-5);
-  test = convert<fahrenheit, kelvin>(451.0);
-  EXPECT_NEAR(505.928, test, 5.0e-4);
-  test = convert<kelvin, celsius>(300.0);
-  EXPECT_NEAR(26.85, test, 5.0e-3);
-  test = convert<celsius, kelvin>(451.0);
-  EXPECT_NEAR(724.15, test, 5.0e-3);
-  test = convert<fahrenheit, celsius>(72.0);
-  EXPECT_NEAR(22.2222, test, 5.0e-5);
-  test = convert<celsius, fahrenheit>(100.0);
-  EXPECT_NEAR(212.0, test, 5.0e-5);
-  test = convert<fahrenheit, celsius>(32.0);
-  EXPECT_NEAR(0.0, test, 5.0e-5);
-  test = convert<celsius, fahrenheit>(0.0);
-  EXPECT_NEAR(32.0, test, 5.0e-5);
-  test = convert<rankine, kelvin>(100.0);
-  EXPECT_NEAR(55.5556, test, 5.0e-5);
-  test = convert<kelvin, rankine>(100.0);
-  EXPECT_NEAR(180.0, test, 5.0e-5);
-  test = convert<fahrenheit, rankine>(100.0);
-  EXPECT_NEAR(559.67, test, 5.0e-5);
-  test = convert<rankine, fahrenheit>(72.0);
-  EXPECT_NEAR(-387.67, test, 5.0e-5);
-  test = convert<reaumur, kelvin>(100.0);
-  EXPECT_NEAR(398.0, test, 5.0e-1);
-  test = convert<reaumur, celsius>(80.0);
-  EXPECT_NEAR(100.0, test, 5.0e-5);
-  test = convert<celsius, reaumur>(212.0);
-  EXPECT_NEAR(169.6, test, 5.0e-2);
-  test = convert<reaumur, fahrenheit>(80.0);
-  EXPECT_NEAR(212.0, test, 5.0e-5);
-  test = convert<fahrenheit, reaumur>(37.0);
-  EXPECT_NEAR(2.222, test, 5.0e-3);
-}
-
-TEST_F(UnitConversion, luminous_intensity) {
-  double test;
-
-  test = convert<candela, millicandela>(72.0);
-  EXPECT_NEAR(72000.0, test, 5.0e-5);
-  test = convert<millicandela, candela>(376.0);
-  EXPECT_NEAR(0.376, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, solid_angle) {
-  double test;
-  bool same;
-
-  same = std::is_same<traits::base_unit_of<steradians>,
-                      traits::base_unit_of<degrees_squared>>::value;
-  EXPECT_TRUE(same);
-
-  test = convert<steradians, steradians>(72.0);
-  EXPECT_NEAR(72.0, test, 5.0e-5);
-  test = convert<steradians, degrees_squared>(1.0);
-  EXPECT_NEAR(3282.8, test, 5.0e-2);
-  test = convert<steradians, spats>(8.0);
-  EXPECT_NEAR(0.636619772367582, test, 5.0e-14);
-  test = convert<degrees_squared, steradians>(3282.8);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<degrees_squared, degrees_squared>(72.0);
-  EXPECT_NEAR(72.0, test, 5.0e-5);
-  test = convert<degrees_squared, spats>(3282.8);
-  EXPECT_NEAR(1.0 / (4 * constants::detail::PI_VAL), test, 5.0e-5);
-  test = convert<spats, steradians>(1.0 / (4 * constants::detail::PI_VAL));
-  EXPECT_NEAR(1.0, test, 5.0e-14);
-  test = convert<spats, degrees_squared>(1.0 / (4 * constants::detail::PI_VAL));
-  EXPECT_NEAR(3282.8, test, 5.0e-2);
-  test = convert<spats, spats>(72.0);
-  EXPECT_NEAR(72.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, frequency) {
-  double test;
-
-  test = convert<hertz, kilohertz>(63000.0);
-  EXPECT_NEAR(63.0, test, 5.0e-5);
-  test = convert<hertz, hertz>(6.3);
-  EXPECT_NEAR(6.3, test, 5.0e-5);
-  test = convert<kilohertz, hertz>(5.0);
-  EXPECT_NEAR(5000.0, test, 5.0e-5);
-  test = convert<megahertz, hertz>(1.0);
-  EXPECT_NEAR(1.0e6, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, velocity) {
-  double test;
-  bool same;
-
-  same = std::is_same<meters_per_second,
-                      unit<std::ratio<1>, category::velocity_unit>>::value;
-  EXPECT_TRUE(same);
-  same = traits::is_convertible_unit<miles_per_hour, meters_per_second>::value;
-  EXPECT_TRUE(same);
-
-  test = convert<meters_per_second, miles_per_hour>(1250.0);
-  EXPECT_NEAR(2796.17, test, 5.0e-3);
-  test = convert<feet_per_second, kilometers_per_hour>(2796.17);
-  EXPECT_NEAR(3068.181418, test, 5.0e-7);
-  test = convert<knots, miles_per_hour>(600.0);
-  EXPECT_NEAR(690.468, test, 5.0e-4);
-  test = convert<miles_per_hour, feet_per_second>(120.0);
-  EXPECT_NEAR(176.0, test, 5.0e-5);
-  test = convert<feet_per_second, meters_per_second>(10.0);
-  EXPECT_NEAR(3.048, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, angular_velocity) {
-  double test;
-  bool same;
-
-  same =
-      std::is_same<radians_per_second,
-                   unit<std::ratio<1>, category::angular_velocity_unit>>::value;
-  EXPECT_TRUE(same);
-  same = traits::is_convertible_unit<rpm, radians_per_second>::value;
-  EXPECT_TRUE(same);
-
-  test = convert<radians_per_second, milliarcseconds_per_year>(1.0);
-  EXPECT_NEAR(6.504e15, test, 1.0e12);
-  test = convert<degrees_per_second, radians_per_second>(1.0);
-  EXPECT_NEAR(0.0174533, test, 5.0e-8);
-  test = convert<rpm, radians_per_second>(1.0);
-  EXPECT_NEAR(0.10471975512, test, 5.0e-13);
-  test = convert<milliarcseconds_per_year, radians_per_second>(1.0);
-  EXPECT_NEAR(1.537e-16, test, 5.0e-20);
-}
-
-TEST_F(UnitConversion, acceleration) {
-  double test;
-
-  test = convert<standard_gravity, meters_per_second_squared>(1.0);
-  EXPECT_NEAR(9.80665, test, 5.0e-10);
-}
-TEST_F(UnitConversion, force) {
-  double test;
-
-  test = convert<units::force::newton, units::force::newton>(1.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<units::force::newton, units::force::pounds>(6.3);
-  EXPECT_NEAR(1.4163, test, 5.0e-5);
-  test = convert<units::force::newton, units::force::dynes>(5.0);
-  EXPECT_NEAR(500000.0, test, 5.0e-5);
-  test = convert<units::force::newtons, units::force::poundals>(2.1);
-  EXPECT_NEAR(15.1893, test, 5.0e-5);
-  test = convert<units::force::newtons, units::force::kiloponds>(173.0);
-  EXPECT_NEAR(17.6411, test, 5.0e-5);
-  test = convert<units::force::poundals, units::force::kiloponds>(21.879);
-  EXPECT_NEAR(0.308451933, test, 5.0e-10);
-}
-
-TEST_F(UnitConversion, area) {
-  double test;
-
-  test = convert<hectares, acres>(6.3);
-  EXPECT_NEAR(15.5676, test, 5.0e-5);
-  test = convert<square_miles, square_kilometers>(10.0);
-  EXPECT_NEAR(25.8999, test, 5.0e-5);
-  test = convert<square_inch, square_meter>(4.0);
-  EXPECT_NEAR(0.00258064, test, 5.0e-9);
-  test = convert<acre, square_foot>(5.0);
-  EXPECT_NEAR(217800.0, test, 5.0e-5);
-  test = convert<square_meter, square_foot>(1.0);
-  EXPECT_NEAR(10.7639, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, pressure) {
-  double test;
-
-  test = convert<pascals, torr>(1.0);
-  EXPECT_NEAR(0.00750062, test, 5.0e-5);
-  test = convert<bar, psi>(2.2);
-  EXPECT_NEAR(31.9083, test, 5.0e-5);
-  test = convert<atmospheres, bar>(4.0);
-  EXPECT_NEAR(4.053, test, 5.0e-5);
-  test = convert<torr, pascals>(800.0);
-  EXPECT_NEAR(106657.89474, test, 5.0e-5);
-  test = convert<psi, atmospheres>(38.0);
-  EXPECT_NEAR(2.58575, test, 5.0e-5);
-  test = convert<psi, pascals>(1.0);
-  EXPECT_NEAR(6894.76, test, 5.0e-3);
-  test = convert<pascals, bar>(0.25);
-  EXPECT_NEAR(2.5e-6, test, 5.0e-5);
-  test = convert<torr, atmospheres>(9.0);
-  EXPECT_NEAR(0.0118421, test, 5.0e-8);
-  test = convert<bar, torr>(12.0);
-  EXPECT_NEAR(9000.74, test, 5.0e-3);
-  test = convert<atmospheres, psi>(1.0);
-  EXPECT_NEAR(14.6959, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, charge) {
-  double test;
-
-  test = convert<coulombs, ampere_hours>(4.0);
-  EXPECT_NEAR(0.00111111, test, 5.0e-9);
-  test = convert<ampere_hours, coulombs>(1.0);
-  EXPECT_NEAR(3600.0, test, 5.0e-6);
-}
-
-TEST_F(UnitConversion, energy) {
-  double test;
-
-  test = convert<joules, calories>(8000.000464);
-  EXPECT_NEAR(1912.046, test, 5.0e-4);
-  test = convert<therms, joules>(12.0);
-  EXPECT_NEAR(1.266e+9, test, 5.0e5);
-  test = convert<megajoules, watt_hours>(100.0);
-  EXPECT_NEAR(27777.778, test, 5.0e-4);
-  test = convert<kilocalories, megajoules>(56.0);
-  EXPECT_NEAR(0.234304, test, 5.0e-7);
-  test = convert<kilojoules, therms>(56.0);
-  EXPECT_NEAR(0.000530904, test, 5.0e-5);
-  test = convert<british_thermal_units, kilojoules>(18.56399995447);
-  EXPECT_NEAR(19.5860568, test, 5.0e-5);
-  test = convert<calories, energy::foot_pounds>(18.56399995447);
-  EXPECT_NEAR(57.28776190423856, test, 5.0e-5);
-  test = convert<megajoules, calories>(1.0);
-  EXPECT_NEAR(239006.0, test, 5.0e-1);
-  test = convert<kilocalories, kilowatt_hours>(2.0);
-  EXPECT_NEAR(0.00232444, test, 5.0e-9);
-  test = convert<therms, kilocalories>(0.1);
-  EXPECT_NEAR(2521.04, test, 5.0e-3);
-  test = convert<watt_hours, megajoules>(67.0);
-  EXPECT_NEAR(0.2412, test, 5.0e-5);
-  test = convert<british_thermal_units, watt_hours>(100.0);
-  EXPECT_NEAR(29.3071, test, 5.0e-5);
-  test = convert<calories, BTU>(100.0);
-  EXPECT_NEAR(0.396567, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, power) {
-  double test;
-
-  test = convert<compound_unit<energy::foot_pounds, inverse<seconds>>, watts>(
-      550.0);
-  EXPECT_NEAR(745.7, test, 5.0e-2);
-  test = convert<watts, gigawatts>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-4);
-  test = convert<microwatts, watts>(200000.0);
-  EXPECT_NEAR(0.2, test, 5.0e-4);
-  test = convert<horsepower, watts>(100.0);
-  EXPECT_NEAR(74570.0, test, 5.0e-1);
-  test = convert<horsepower, megawatts>(5.0);
-  EXPECT_NEAR(0.0037284994, test, 5.0e-7);
-  test = convert<kilowatts, horsepower>(232.0);
-  EXPECT_NEAR(311.117, test, 5.0e-4);
-  test = convert<milliwatts, horsepower>(1001.0);
-  EXPECT_NEAR(0.001342363, test, 5.0e-9);
-}
-
-TEST_F(UnitConversion, voltage) {
-  double test;
-
-  test = convert<volts, millivolts>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picovolts, volts>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanovolts, volts>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microvolts, volts>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millivolts, volts>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilovolts, volts>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megavolts, volts>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigavolts, volts>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<statvolts, volts>(299.792458);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millivolts, statvolts>(1000.0);
-  EXPECT_NEAR(299.792458, test, 5.0e-5);
-  test = convert<abvolts, nanovolts>(0.1);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microvolts, abvolts>(0.01);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, capacitance) {
-  double test;
-
-  test = convert<farads, millifarads>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picofarads, farads>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanofarads, farads>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microfarads, farads>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millifarads, farads>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilofarads, farads>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megafarads, farads>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigafarads, farads>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, impedance) {
-  double test;
-
-  test = convert<ohms, milliohms>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picoohms, ohms>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanoohms, ohms>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microohms, ohms>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<milliohms, ohms>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kiloohms, ohms>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megaohms, ohms>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigaohms, ohms>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, conductance) {
-  double test;
-
-  test = convert<siemens, millisiemens>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picosiemens, siemens>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanosiemens, siemens>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microsiemens, siemens>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millisiemens, siemens>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilosiemens, siemens>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megasiemens, siemens>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigasiemens, siemens>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, magnetic_flux) {
-  double test;
-
-  test = convert<webers, milliwebers>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picowebers, webers>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanowebers, webers>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microwebers, webers>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<milliwebers, webers>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilowebers, webers>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megawebers, webers>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigawebers, webers>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<maxwells, webers>(100000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanowebers, maxwells>(10.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, magnetic_field_strength) {
-  double test;
-
-  test = convert<teslas, milliteslas>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picoteslas, teslas>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanoteslas, teslas>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microteslas, teslas>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<milliteslas, teslas>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kiloteslas, teslas>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megateslas, teslas>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigateslas, teslas>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gauss, teslas>(10000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanoteslas, gauss>(100000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, inductance) {
-  double test;
-
-  test = convert<henries, millihenries>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picohenries, henries>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanohenries, henries>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microhenries, henries>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millihenries, henries>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilohenries, henries>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megahenries, henries>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigahenries, henries>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, luminous_flux) {
-  double test;
-
-  test = convert<lumens, millilumens>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picolumens, lumens>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanolumens, lumens>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microlumens, lumens>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millilumens, lumens>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilolumens, lumens>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megalumens, lumens>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigalumens, lumens>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, illuminance) {
-  double test;
-
-  test = convert<luxes, milliluxes>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picoluxes, luxes>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanoluxes, luxes>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microluxes, luxes>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<milliluxes, luxes>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kiloluxes, luxes>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megaluxes, luxes>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigaluxes, luxes>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-
-  test = convert<footcandles, luxes>(0.092903);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<lux, lumens_per_square_inch>(1550.0031000062);
-  EXPECT_NEAR(1.0, test, 5.0e-13);
-  test = convert<phots, luxes>(0.0001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, radiation) {
-  double test;
-
-  test = convert<becquerels, millibecquerels>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picobecquerels, becquerels>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanobecquerels, becquerels>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microbecquerels, becquerels>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millibecquerels, becquerels>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilobecquerels, becquerels>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megabecquerels, becquerels>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigabecquerels, becquerels>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-
-  test = convert<grays, milligrays>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picograys, grays>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanograys, grays>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<micrograys, grays>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<milligrays, grays>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilograys, grays>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megagrays, grays>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigagrays, grays>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-
-  test = convert<sieverts, millisieverts>(10.0);
-  EXPECT_NEAR(10000.0, test, 5.0e-5);
-  test = convert<picosieverts, sieverts>(1000000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<nanosieverts, sieverts>(1000000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<microsieverts, sieverts>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<millisieverts, sieverts>(1000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<kilosieverts, sieverts>(0.001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<megasieverts, sieverts>(0.000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<gigasieverts, sieverts>(0.000000001);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-
-  test = convert<becquerels, curies>(37.0e9);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<becquerels, rutherfords>(1000000.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<rads, grays>(100.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, torque) {
-  double test;
-
-  test = convert<torque::foot_pounds, newton_meter>(1.0);
-  EXPECT_NEAR(1.355817948, test, 5.0e-5);
-  test = convert<inch_pounds, newton_meter>(1.0);
-  EXPECT_NEAR(0.112984829, test, 5.0e-5);
-  test = convert<foot_poundals, newton_meter>(1.0);
-  EXPECT_NEAR(4.214011009e-2, test, 5.0e-5);
-  test = convert<meter_kilograms, newton_meter>(1.0);
-  EXPECT_NEAR(9.80665, test, 5.0e-5);
-  test = convert<inch_pound, meter_kilogram>(86.79616930855788);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<foot_poundals, inch_pound>(2.681170713);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, volume) {
-  double test;
-
-  test = convert<cubic_meters, cubic_meter>(1.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<cubic_millimeters, cubic_meter>(1.0);
-  EXPECT_NEAR(1.0e-9, test, 5.0e-5);
-  test = convert<cubic_kilometers, cubic_meter>(1.0);
-  EXPECT_NEAR(1.0e9, test, 5.0e-5);
-  test = convert<liters, cubic_meter>(1.0);
-  EXPECT_NEAR(0.001, test, 5.0e-5);
-  test = convert<milliliters, cubic_meter>(1.0);
-  EXPECT_NEAR(1.0e-6, test, 5.0e-5);
-  test = convert<cubic_inches, cubic_meter>(1.0);
-  EXPECT_NEAR(1.6387e-5, test, 5.0e-10);
-  test = convert<cubic_feet, cubic_meter>(1.0);
-  EXPECT_NEAR(0.0283168, test, 5.0e-8);
-  test = convert<cubic_yards, cubic_meter>(1.0);
-  EXPECT_NEAR(0.764555, test, 5.0e-7);
-  test = convert<cubic_miles, cubic_meter>(1.0);
-  EXPECT_NEAR(4.168e+9, test, 5.0e5);
-  test = convert<gallons, cubic_meter>(1.0);
-  EXPECT_NEAR(0.00378541, test, 5.0e-8);
-  test = convert<quarts, cubic_meter>(1.0);
-  EXPECT_NEAR(0.000946353, test, 5.0e-10);
-  test = convert<pints, cubic_meter>(1.0);
-  EXPECT_NEAR(0.000473176, test, 5.0e-10);
-  test = convert<cups, cubic_meter>(1.0);
-  EXPECT_NEAR(0.00024, test, 5.0e-6);
-  test = convert<volume::fluid_ounces, cubic_meter>(1.0);
-  EXPECT_NEAR(2.9574e-5, test, 5.0e-5);
-  test = convert<barrels, cubic_meter>(1.0);
-  EXPECT_NEAR(0.158987294928, test, 5.0e-13);
-  test = convert<bushels, cubic_meter>(1.0);
-  EXPECT_NEAR(0.0352391, test, 5.0e-8);
-  test = convert<cords, cubic_meter>(1.0);
-  EXPECT_NEAR(3.62456, test, 5.0e-6);
-  test = convert<cubic_fathoms, cubic_meter>(1.0);
-  EXPECT_NEAR(6.11644, test, 5.0e-6);
-  test = convert<tablespoons, cubic_meter>(1.0);
-  EXPECT_NEAR(1.4787e-5, test, 5.0e-10);
-  test = convert<teaspoons, cubic_meter>(1.0);
-  EXPECT_NEAR(4.9289e-6, test, 5.0e-11);
-  test = convert<pinches, cubic_meter>(1.0);
-  EXPECT_NEAR(616.11519921875e-9, test, 5.0e-20);
-  test = convert<dashes, cubic_meter>(1.0);
-  EXPECT_NEAR(308.057599609375e-9, test, 5.0e-20);
-  test = convert<drops, cubic_meter>(1.0);
-  EXPECT_NEAR(82.14869322916e-9, test, 5.0e-9);
-  test = convert<fifths, cubic_meter>(1.0);
-  EXPECT_NEAR(0.00075708236, test, 5.0e-12);
-  test = convert<drams, cubic_meter>(1.0);
-  EXPECT_NEAR(3.69669e-6, test, 5.0e-12);
-  test = convert<gills, cubic_meter>(1.0);
-  EXPECT_NEAR(0.000118294, test, 5.0e-10);
-  test = convert<pecks, cubic_meter>(1.0);
-  EXPECT_NEAR(0.00880977, test, 5.0e-9);
-  test = convert<sacks, cubic_meter>(9.4591978);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<shots, cubic_meter>(1.0);
-  EXPECT_NEAR(4.43603e-5, test, 5.0e-11);
-  test = convert<strikes, cubic_meter>(1.0);
-  EXPECT_NEAR(0.07047814033376, test, 5.0e-5);
-  test = convert<volume::fluid_ounces, milliliters>(1.0);
-  EXPECT_NEAR(29.5735, test, 5.0e-5);
-}
-
-TEST_F(UnitConversion, density) {
-  double test;
-
-  test = convert<kilograms_per_cubic_meter, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(1.0, test, 5.0e-5);
-  test = convert<grams_per_milliliter, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(1000.0, test, 5.0e-5);
-  test = convert<kilograms_per_liter, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(1000.0, test, 5.0e-5);
-  test = convert<ounces_per_cubic_foot, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(1.001153961, test, 5.0e-10);
-  test = convert<ounces_per_cubic_inch, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(1.729994044e3, test, 5.0e-7);
-  test = convert<ounces_per_gallon, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(7.489151707, test, 5.0e-10);
-  test = convert<pounds_per_cubic_foot, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(16.01846337, test, 5.0e-9);
-  test = convert<pounds_per_cubic_inch, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(2.767990471e4, test, 5.0e-6);
-  test = convert<pounds_per_gallon, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(119.8264273, test, 5.0e-8);
-  test = convert<slugs_per_cubic_foot, kilograms_per_cubic_meter>(1.0);
-  EXPECT_NEAR(515.3788184, test, 5.0e-6);
-}
-
-TEST_F(UnitConversion, concentration) {
-  double test;
-
-  test = ppm_t(1.0);
-  EXPECT_NEAR(1.0e-6, test, 5.0e-12);
-  test = ppb_t(1.0);
-  EXPECT_NEAR(1.0e-9, test, 5.0e-12);
-  test = ppt_t(1.0);
-  EXPECT_NEAR(1.0e-12, test, 5.0e-12);
-  test = percent_t(18.0);
-  EXPECT_NEAR(0.18, test, 5.0e-12);
-}
-
-TEST_F(UnitConversion, data) {
-  EXPECT_EQ(8, (convert<byte, bit>(1)));
-
-  EXPECT_EQ(1000, (convert<kilobytes, bytes>(1)));
-  EXPECT_EQ(1000, (convert<megabytes, kilobytes>(1)));
-  EXPECT_EQ(1000, (convert<gigabytes, megabytes>(1)));
-  EXPECT_EQ(1000, (convert<terabytes, gigabytes>(1)));
-  EXPECT_EQ(1000, (convert<petabytes, terabytes>(1)));
-  EXPECT_EQ(1000, (convert<exabytes, petabytes>(1)));
-
-  EXPECT_EQ(1024, (convert<kibibytes, bytes>(1)));
-  EXPECT_EQ(1024, (convert<mebibytes, kibibytes>(1)));
-  EXPECT_EQ(1024, (convert<gibibytes, mebibytes>(1)));
-  EXPECT_EQ(1024, (convert<tebibytes, gibibytes>(1)));
-  EXPECT_EQ(1024, (convert<pebibytes, tebibytes>(1)));
-  EXPECT_EQ(1024, (convert<exbibytes, pebibytes>(1)));
-
-  EXPECT_EQ(93750000, (convert<gigabytes, kibibits>(12)));
-
-  EXPECT_EQ(1000, (convert<kilobits, bits>(1)));
-  EXPECT_EQ(1000, (convert<megabits, kilobits>(1)));
-  EXPECT_EQ(1000, (convert<gigabits, megabits>(1)));
-  EXPECT_EQ(1000, (convert<terabits, gigabits>(1)));
-  EXPECT_EQ(1000, (convert<petabits, terabits>(1)));
-  EXPECT_EQ(1000, (convert<exabits, petabits>(1)));
-
-  EXPECT_EQ(1024, (convert<kibibits, bits>(1)));
-  EXPECT_EQ(1024, (convert<mebibits, kibibits>(1)));
-  EXPECT_EQ(1024, (convert<gibibits, mebibits>(1)));
-  EXPECT_EQ(1024, (convert<tebibits, gibibits>(1)));
-  EXPECT_EQ(1024, (convert<pebibits, tebibits>(1)));
-  EXPECT_EQ(1024, (convert<exbibits, pebibits>(1)));
-
-  // Source: https://en.wikipedia.org/wiki/Binary_prefix
-  EXPECT_NEAR(percent_t(2.4), kibibyte_t(1) / kilobyte_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(4.9), mebibyte_t(1) / megabyte_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(7.4), gibibyte_t(1) / gigabyte_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(10.0), tebibyte_t(1) / terabyte_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(12.6), pebibyte_t(1) / petabyte_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(15.3), exbibyte_t(1) / exabyte_t(1) - 1, 0.005);
-}
-
-TEST_F(UnitConversion, data_transfer_rate) {
-  EXPECT_EQ(8, (convert<bytes_per_second, bits_per_second>(1)));
-
-  EXPECT_EQ(1000, (convert<kilobytes_per_second, bytes_per_second>(1)));
-  EXPECT_EQ(1000, (convert<megabytes_per_second, kilobytes_per_second>(1)));
-  EXPECT_EQ(1000, (convert<gigabytes_per_second, megabytes_per_second>(1)));
-  EXPECT_EQ(1000, (convert<terabytes_per_second, gigabytes_per_second>(1)));
-  EXPECT_EQ(1000, (convert<petabytes_per_second, terabytes_per_second>(1)));
-  EXPECT_EQ(1000, (convert<exabytes_per_second, petabytes_per_second>(1)));
-
-  EXPECT_EQ(1024, (convert<kibibytes_per_second, bytes_per_second>(1)));
-  EXPECT_EQ(1024, (convert<mebibytes_per_second, kibibytes_per_second>(1)));
-  EXPECT_EQ(1024, (convert<gibibytes_per_second, mebibytes_per_second>(1)));
-  EXPECT_EQ(1024, (convert<tebibytes_per_second, gibibytes_per_second>(1)));
-  EXPECT_EQ(1024, (convert<pebibytes_per_second, tebibytes_per_second>(1)));
-  EXPECT_EQ(1024, (convert<exbibytes_per_second, pebibytes_per_second>(1)));
-
-  EXPECT_EQ(93750000, (convert<gigabytes_per_second, kibibits_per_second>(12)));
-
-  EXPECT_EQ(1000, (convert<kilobits_per_second, bits_per_second>(1)));
-  EXPECT_EQ(1000, (convert<megabits_per_second, kilobits_per_second>(1)));
-  EXPECT_EQ(1000, (convert<gigabits_per_second, megabits_per_second>(1)));
-  EXPECT_EQ(1000, (convert<terabits_per_second, gigabits_per_second>(1)));
-  EXPECT_EQ(1000, (convert<petabits_per_second, terabits_per_second>(1)));
-  EXPECT_EQ(1000, (convert<exabits_per_second, petabits_per_second>(1)));
-
-  EXPECT_EQ(1024, (convert<kibibits_per_second, bits_per_second>(1)));
-  EXPECT_EQ(1024, (convert<mebibits_per_second, kibibits_per_second>(1)));
-  EXPECT_EQ(1024, (convert<gibibits_per_second, mebibits_per_second>(1)));
-  EXPECT_EQ(1024, (convert<tebibits_per_second, gibibits_per_second>(1)));
-  EXPECT_EQ(1024, (convert<pebibits_per_second, tebibits_per_second>(1)));
-  EXPECT_EQ(1024, (convert<exbibits_per_second, pebibits_per_second>(1)));
-
-  // Source: https://en.wikipedia.org/wiki/Binary_prefix
-  EXPECT_NEAR(percent_t(2.4),
-              kibibytes_per_second_t(1) / kilobytes_per_second_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(4.9),
-              mebibytes_per_second_t(1) / megabytes_per_second_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(7.4),
-              gibibytes_per_second_t(1) / gigabytes_per_second_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(10.0),
-              tebibytes_per_second_t(1) / terabytes_per_second_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(12.6),
-              pebibytes_per_second_t(1) / petabytes_per_second_t(1) - 1, 0.005);
-  EXPECT_NEAR(percent_t(15.3),
-              exbibytes_per_second_t(1) / exabytes_per_second_t(1) - 1, 0.005);
-}
-
-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);
-
-  // implicit conversion/arithmetic
-  EXPECT_NEAR(3.14159, constants::pi, 5.0e-6);
-  EXPECT_NEAR(6.28318531, (2 * constants::pi), 5.0e-9);
-  EXPECT_NEAR(6.28318531, (constants::pi + constants::pi), 5.0e-9);
-  EXPECT_NEAR(0.0, (constants::pi - constants::pi), 5.0e-9);
-  EXPECT_NEAR(31.00627668, units::math::cpow<3>(constants::pi), 5.0e-10);
-  EXPECT_NEAR(0.0322515344, (1.0 / units::math::cpow<3>(constants::pi)),
-              5.0e-11);
-  EXPECT_TRUE(constants::detail::PI_VAL == constants::pi);
-  EXPECT_TRUE(1.0 != constants::pi);
-  EXPECT_TRUE(4.0 > constants::pi);
-  EXPECT_TRUE(3.0 < constants::pi);
-  EXPECT_TRUE(constants::pi > 3.0);
-  EXPECT_TRUE(constants::pi < 4.0);
-
-  // explicit conversion
-  EXPECT_NEAR(3.14159, constants::pi.to<double>(), 5.0e-6);
-
-  // auto multiplication
-  EXPECT_TRUE(
-      (std::is_same<meter_t, decltype(constants::pi * meter_t(1))>::value));
-  EXPECT_TRUE(
-      (std::is_same<meter_t, decltype(meter_t(1) * constants::pi)>::value));
-
-  EXPECT_NEAR(constants::detail::PI_VAL,
-              (constants::pi * meter_t(1)).to<double>(), 5.0e-10);
-  EXPECT_NEAR(constants::detail::PI_VAL,
-              (meter_t(1) * constants::pi).to<double>(), 5.0e-10);
-
-  // explicit multiplication
-  meter_t a = constants::pi * meter_t(1);
-  meter_t b = meter_t(1) * constants::pi;
-
-  EXPECT_NEAR(constants::detail::PI_VAL, a.to<double>(), 5.0e-10);
-  EXPECT_NEAR(constants::detail::PI_VAL, b.to<double>(), 5.0e-10);
-
-  // auto division
-  EXPECT_TRUE(
-      (std::is_same<hertz_t, decltype(constants::pi / second_t(1))>::value));
-  EXPECT_TRUE(
-      (std::is_same<second_t, decltype(second_t(1) / constants::pi)>::value));
-
-  EXPECT_NEAR(constants::detail::PI_VAL,
-              (constants::pi / second_t(1)).to<double>(), 5.0e-10);
-  EXPECT_NEAR(1.0 / constants::detail::PI_VAL,
-              (second_t(1) / constants::pi).to<double>(), 5.0e-10);
-
-  // explicit
-  hertz_t c = constants::pi / second_t(1);
-  second_t d = second_t(1) / constants::pi;
-
-  EXPECT_NEAR(constants::detail::PI_VAL, c.to<double>(), 5.0e-10);
-  EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.to<double>(), 5.0e-10);
-}
-
-TEST_F(UnitConversion, constants) {
-  // Source: NIST "2014 CODATA recommended values"
-  EXPECT_NEAR(299792458, constants::c(), 5.0e-9);
-  EXPECT_NEAR(6.67408e-11, constants::G(), 5.0e-17);
-  EXPECT_NEAR(6.626070040e-34, constants::h(), 5.0e-44);
-  EXPECT_NEAR(1.2566370614e-6, constants::mu0(), 5.0e-17);
-  EXPECT_NEAR(8.854187817e-12, constants::epsilon0(), 5.0e-21);
-  EXPECT_NEAR(376.73031346177, constants::Z0(), 5.0e-12);
-  EXPECT_NEAR(8987551787.3681764, constants::k_e(), 5.0e-6);
-  EXPECT_NEAR(1.6021766208e-19, constants::e(), 5.0e-29);
-  EXPECT_NEAR(9.10938356e-31, constants::m_e(), 5.0e-40);
-  EXPECT_NEAR(1.672621898e-27, constants::m_p(), 5.0e-37);
-  EXPECT_NEAR(9.274009994e-24, constants::mu_B(), 5.0e-32);
-  EXPECT_NEAR(6.022140857e23, constants::N_A(), 5.0e14);
-  EXPECT_NEAR(8.3144598, constants::R(), 5.0e-8);
-  EXPECT_NEAR(1.38064852e-23, constants::k_B(), 5.0e-31);
-  EXPECT_NEAR(96485.33289, constants::F(), 5.0e-5);
-  EXPECT_NEAR(5.670367e-8, constants::sigma(), 5.0e-14);
-}
-
-TEST_F(UnitConversion, std_chrono) {
-  nanosecond_t a = std::chrono::nanoseconds(10);
-  EXPECT_EQ(nanosecond_t(10), a);
-  microsecond_t b = std::chrono::microseconds(10);
-  EXPECT_EQ(microsecond_t(10), b);
-  millisecond_t c = std::chrono::milliseconds(10);
-  EXPECT_EQ(millisecond_t(10), c);
-  second_t d = std::chrono::seconds(1);
-  EXPECT_EQ(second_t(1), d);
-  minute_t e = std::chrono::minutes(120);
-  EXPECT_EQ(minute_t(120), e);
-  hour_t f = std::chrono::hours(2);
-  EXPECT_EQ(hour_t(2), f);
-
-  std::chrono::nanoseconds g = nanosecond_t(100);
-  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(g).count(),
-            100);
-  std::chrono::nanoseconds h = microsecond_t(2);
-  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(h).count(),
-            2000);
-  std::chrono::nanoseconds i = millisecond_t(1);
-  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(i).count(),
-            1000000);
-  std::chrono::nanoseconds j = second_t(1);
-  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(j).count(),
-            1000000000);
-  std::chrono::nanoseconds k = minute_t(1);
-  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(k).count(),
-            60000000000);
-  std::chrono::nanoseconds l = hour_t(1);
-  EXPECT_EQ(std::chrono::duration_cast<std::chrono::nanoseconds>(l).count(),
-            3600000000000);
-}
-
-TEST_F(UnitConversion, squaredTemperature) {
-  using squared_celsius = units::compound_unit<squared<celsius>>;
-  using squared_celsius_t = units::unit_t<squared_celsius>;
-  const squared_celsius_t right(100);
-  const celsius_t rootRight = units::math::sqrt(right);
-  EXPECT_EQ(celsius_t(10), rootRight);
-}
-
-TEST_F(UnitMath, min) {
-  meter_t a(1);
-  foot_t c(1);
-  EXPECT_EQ(c, math::min(a, c));
-}
-
-TEST_F(UnitMath, max) {
-  meter_t a(1);
-  foot_t c(1);
-  EXPECT_EQ(a, math::max(a, c));
-}
-
-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_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_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_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));
-  EXPECT_NEAR(angle::radian_t(2).to<double>(),
-              acos(scalar_t(-0.41614683654)).to<double>(), 5.0e-11);
-  EXPECT_NEAR(
-      angle::degree_t(135).to<double>(),
-      angle::degree_t(acos(scalar_t(-0.70710678118654752440084436210485)))
-          .to<double>(),
-      5.0e-12);
-}
-
-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));
-  EXPECT_NEAR(angle::radian_t(1.14159265).to<double>(),
-              asin(scalar_t(0.90929742682)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(
-      angle::degree_t(45).to<double>(),
-      angle::degree_t(asin(scalar_t(0.70710678118654752440084436210485)))
-          .to<double>(),
-      5.0e-12);
-}
-
-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));
-  EXPECT_NEAR(angle::radian_t(-1.14159265).to<double>(),
-              atan(scalar_t(-2.18503986326)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(angle::degree_t(-45).to<double>(),
-              angle::degree_t(atan(scalar_t(-1.0))).to<double>(), 5.0e-12);
-}
-
-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_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).to<double>(),
-              atan2(scalar_t(2), scalar_t(2)).to<double>(), 5.0e-12);
-  EXPECT_NEAR(
-      angle::degree_t(45).to<double>(),
-      angle::degree_t(atan2(scalar_t(2), scalar_t(2))).to<double>(),
-      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_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).to<double>(),
-              atan2(scalar_t(1), scalar_t(std::sqrt(3))).to<double>(),
-              5.0e-12);
-  EXPECT_NEAR(angle::degree_t(30).to<double>(),
-              angle::degree_t(atan2(scalar_t(1), scalar_t(std::sqrt(3))))
-                  .to<double>(),
-              5.0e-12);
-}
-
-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_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_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_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_NEAR(angle::radian_t(1.316957896924817).to<double>(),
-              acosh(scalar_t(2.0)).to<double>(), 5.0e-11);
-  EXPECT_NEAR(angle::degree_t(75.456129290216893).to<double>(),
-              angle::degree_t(acosh(scalar_t(2.0))).to<double>(), 5.0e-12);
-}
-
-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_NEAR(angle::radian_t(1.443635475178810).to<double>(),
-              asinh(scalar_t(2)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(angle::degree_t(82.714219883108939).to<double>(),
-              angle::degree_t(asinh(scalar_t(2))).to<double>(), 5.0e-12);
-}
-
-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_NEAR(angle::radian_t(0.549306144334055).to<double>(),
-              atanh(scalar_t(0.5)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(angle::degree_t(31.472923730945389).to<double>(),
-              angle::degree_t(atanh(scalar_t(0.5))).to<double>(), 5.0e-12);
-}
-
-TEST_F(UnitMath, exp) {
-  double val = 10.0;
-  EXPECT_EQ(std::exp(val), exp(scalar_t(val)));
-}
-
-TEST_F(UnitMath, log) {
-  double val = 100.0;
-  EXPECT_EQ(std::log(val), log(scalar_t(val)));
-}
-
-TEST_F(UnitMath, log10) {
-  double val = 100.0;
-  EXPECT_EQ(std::log10(val), log10(scalar_t(val)));
-}
-
-TEST_F(UnitMath, modf) {
-  double val = 100.0;
-  double modfr1;
-  scalar_t modfr2;
-  EXPECT_EQ(std::modf(val, &modfr1), modf(scalar_t(val), &modfr2));
-  EXPECT_EQ(modfr1, modfr2);
-}
-
-TEST_F(UnitMath, exp2) {
-  double val = 10.0;
-  EXPECT_EQ(std::exp2(val), exp2(scalar_t(val)));
-}
-
-TEST_F(UnitMath, expm1) {
-  double val = 10.0;
-  EXPECT_EQ(std::expm1(val), expm1(scalar_t(val)));
-}
-
-TEST_F(UnitMath, log1p) {
-  double val = 10.0;
-  EXPECT_EQ(std::log1p(val), log1p(scalar_t(val)));
-}
-
-TEST_F(UnitMath, log2) {
-  double val = 10.0;
-  EXPECT_EQ(std::log2(val), log2(scalar_t(val)));
-}
-
-TEST_F(UnitMath, pow) {
-  bool isSame;
-  meter_t value(10.0);
-
-  auto sq = pow<2>(value);
-  EXPECT_NEAR(100.0, sq(), 5.0e-2);
-  isSame = std::is_same<decltype(sq), square_meter_t>::value;
-  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;
-  EXPECT_TRUE(isSame);
-
-  auto fourth = pow<4>(value);
-  EXPECT_NEAR(10000.0, fourth(), 5.0e-2);
-  isSame = std::is_same<
-      decltype(fourth),
-      unit_t<compound_unit<squared<meter>, squared<meter>>>>::value;
-  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_NEAR(meter_t(2.0).to<double>(),
-              sqrt(square_meter_t(4.0)).to<double>(), 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_NEAR(angle::radian_t(4.0).to<double>(),
-              sqrt(steradian_t(16.0)).to<double>(), 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));
-
-  // for rational conversion (i.e. no integral root) let's check a bunch of
-  // different ways this could go wrong
-  foot_t resultFt = sqrt(square_foot_t(10.0));
-  EXPECT_NEAR(foot_t(3.16227766017).to<double>(),
-              sqrt(square_foot_t(10.0)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(foot_t(3.16227766017).to<double>(), resultFt.to<double>(),
-              5.0e-9);
-  EXPECT_EQ(resultFt, sqrt(square_foot_t(10.0)));
-}
-
-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_NEAR(meter_t(5.0).to<double>(),
-              (hypot(meter_t(3.0), meter_t(4.0))).to<double>(), 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_NEAR(foot_t(5.0).to<double>(),
-              (hypot(foot_t(3.0), meter_t(1.2192))).to<double>(), 5.0e-9);
-}
-
-TEST_F(UnitMath, ceil) {
-  double val = 101.1;
-  EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).to<double>());
-  EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
-                            typename std::decay<decltype(
-                                ceil(meter_t(val)))>::type>::value));
-}
-
-TEST_F(UnitMath, floor) {
-  double val = 101.1;
-  EXPECT_EQ(std::floor(val), floor(scalar_t(val)));
-}
-
-TEST_F(UnitMath, fmod) {
-  EXPECT_EQ(std::fmod(100.0, 101.2),
-            fmod(meter_t(100.0), meter_t(101.2)).to<double>());
-}
-
-TEST_F(UnitMath, trunc) {
-  double val = 101.1;
-  EXPECT_EQ(std::trunc(val), trunc(scalar_t(val)));
-}
-
-TEST_F(UnitMath, round) {
-  double val = 101.1;
-  EXPECT_EQ(std::round(val), round(scalar_t(val)));
-}
-
-TEST_F(UnitMath, copysign) {
-  double sign = -1;
-  meter_t val(5.0);
-  EXPECT_EQ(meter_t(-5.0), copysign(val, sign));
-  EXPECT_EQ(meter_t(-5.0), copysign(val, angle::radian_t(sign)));
-}
-
-TEST_F(UnitMath, fdim) {
-  EXPECT_EQ(meter_t(0.0), fdim(meter_t(8.0), meter_t(10.0)));
-  EXPECT_EQ(meter_t(2.0), fdim(meter_t(10.0), meter_t(8.0)));
-  EXPECT_NEAR(meter_t(9.3904).to<double>(),
-              fdim(meter_t(10.0), foot_t(2.0)).to<double>(),
-              5.0e-320);  // not sure why they aren't comparing exactly equal,
-                          // but clearly they are.
-}
-
-TEST_F(UnitMath, fmin) {
-  EXPECT_EQ(meter_t(8.0), fmin(meter_t(8.0), meter_t(10.0)));
-  EXPECT_EQ(meter_t(8.0), fmin(meter_t(10.0), meter_t(8.0)));
-  EXPECT_EQ(foot_t(2.0), fmin(meter_t(10.0), foot_t(2.0)));
-}
-
-TEST_F(UnitMath, fmax) {
-  EXPECT_EQ(meter_t(10.0), fmax(meter_t(8.0), meter_t(10.0)));
-  EXPECT_EQ(meter_t(10.0), fmax(meter_t(10.0), meter_t(8.0)));
-  EXPECT_EQ(meter_t(10.0), fmax(meter_t(10.0), foot_t(2.0)));
-}
-
-TEST_F(UnitMath, fabs) {
-  EXPECT_EQ(meter_t(10.0), fabs(meter_t(-10.0)));
-  EXPECT_EQ(meter_t(10.0), fabs(meter_t(10.0)));
-}
-
-TEST_F(UnitMath, abs) {
-  EXPECT_EQ(meter_t(10.0), abs(meter_t(-10.0)));
-  EXPECT_EQ(meter_t(10.0), abs(meter_t(10.0)));
-}
-
-TEST_F(UnitMath, fma) {
-  meter_t x(2.0);
-  meter_t y(3.0);
-  square_meter_t z(1.0);
-  EXPECT_EQ(square_meter_t(7.0), fma(x, y, z));
-}
-
-// Constexpr
-#if !defined(_MSC_VER) || _MSC_VER > 1800
-TEST_F(Constexpr, construction) {
-  constexpr meter_t result0(0);
-  constexpr auto result1 = make_unit<meter_t>(1);
-  constexpr auto result2 = meter_t(2);
-
-  EXPECT_EQ(meter_t(0), result0);
-  EXPECT_EQ(meter_t(1), result1);
-  EXPECT_EQ(meter_t(2), result2);
-
-  EXPECT_TRUE(noexcept(result0));
-  EXPECT_TRUE(noexcept(result1));
-  EXPECT_TRUE(noexcept(result2));
-}
-
-TEST_F(Constexpr, constants) {
-  EXPECT_TRUE(noexcept(constants::c()));
-  EXPECT_TRUE(noexcept(constants::G()));
-  EXPECT_TRUE(noexcept(constants::h()));
-  EXPECT_TRUE(noexcept(constants::mu0()));
-  EXPECT_TRUE(noexcept(constants::epsilon0()));
-  EXPECT_TRUE(noexcept(constants::Z0()));
-  EXPECT_TRUE(noexcept(constants::k_e()));
-  EXPECT_TRUE(noexcept(constants::e()));
-  EXPECT_TRUE(noexcept(constants::m_e()));
-  EXPECT_TRUE(noexcept(constants::m_p()));
-  EXPECT_TRUE(noexcept(constants::mu_B()));
-  EXPECT_TRUE(noexcept(constants::N_A()));
-  EXPECT_TRUE(noexcept(constants::R()));
-  EXPECT_TRUE(noexcept(constants::k_B()));
-  EXPECT_TRUE(noexcept(constants::F()));
-  EXPECT_TRUE(noexcept(constants::sigma()));
-}
-
-TEST_F(Constexpr, arithmetic) {
-  constexpr auto result0(1_m + 1_m);
-  constexpr auto result1(1_m - 1_m);
-  constexpr auto result2(1_m * 1_m);
-  constexpr auto result3(1_m / 1_m);
-  constexpr auto result4(meter_t(1) + meter_t(1));
-  constexpr auto result5(meter_t(1) - meter_t(1));
-  constexpr auto result6(meter_t(1) * meter_t(1));
-  constexpr auto result7(meter_t(1) / meter_t(1));
-  constexpr auto result8(units::math::cpow<2>(meter_t(2)));
-  constexpr auto result9 = units::math::cpow<3>(2_m);
-  constexpr auto result10 = 2_m * 2_m;
-
-  EXPECT_TRUE(noexcept(result0));
-  EXPECT_TRUE(noexcept(result1));
-  EXPECT_TRUE(noexcept(result2));
-  EXPECT_TRUE(noexcept(result3));
-  EXPECT_TRUE(noexcept(result4));
-  EXPECT_TRUE(noexcept(result5));
-  EXPECT_TRUE(noexcept(result6));
-  EXPECT_TRUE(noexcept(result7));
-  EXPECT_TRUE(noexcept(result8));
-  EXPECT_TRUE(noexcept(result9));
-  EXPECT_TRUE(noexcept(result10));
-
-  EXPECT_EQ(8_cu_m, result9);
-  EXPECT_EQ(4_sq_m, result10);
-}
-
-TEST_F(Constexpr, realtional) {
-  constexpr bool equalityTrue = (1_m == 1_m);
-  constexpr bool equalityFalse = (1_m == 2_m);
-  constexpr bool lessThanTrue = (1_m < 2_m);
-  constexpr bool lessThanFalse = (1_m < 1_m);
-  constexpr bool lessThanEqualTrue1 = (1_m <= 1_m);
-  constexpr bool lessThanEqualTrue2 = (1_m <= 2_m);
-  constexpr bool lessThanEqualFalse = (1_m < 0_m);
-  constexpr bool greaterThanTrue = (2_m > 1_m);
-  constexpr bool greaterThanFalse = (2_m > 2_m);
-  constexpr bool greaterThanEqualTrue1 = (2_m >= 1_m);
-  constexpr bool greaterThanEqualTrue2 = (2_m >= 2_m);
-  constexpr bool greaterThanEqualFalse = (2_m > 3_m);
-
-  EXPECT_TRUE(equalityTrue);
-  EXPECT_TRUE(lessThanTrue);
-  EXPECT_TRUE(lessThanEqualTrue1);
-  EXPECT_TRUE(lessThanEqualTrue2);
-  EXPECT_TRUE(greaterThanTrue);
-  EXPECT_TRUE(greaterThanEqualTrue1);
-  EXPECT_TRUE(greaterThanEqualTrue2);
-  EXPECT_FALSE(equalityFalse);
-  EXPECT_FALSE(lessThanFalse);
-  EXPECT_FALSE(lessThanEqualFalse);
-  EXPECT_FALSE(greaterThanFalse);
-  EXPECT_FALSE(greaterThanEqualFalse);
-}
-
-TEST_F(Constexpr, stdArray) {
-  constexpr std::array<meter_t, 5> arr = {0_m, 1_m, 2_m, 3_m, 4_m};
-  constexpr bool equal = (arr[3] == 3_m);
-  EXPECT_TRUE(equal);
-}
-
-#endif
-
-TEST_F(CompileTimeArithmetic, unit_value_t) {
-  typedef unit_value_t<meters, 3, 2> mRatio;
-  EXPECT_EQ(meter_t(1.5), mRatio::value());
-}
-
-TEST_F(CompileTimeArithmetic, is_unit_value_t) {
-  typedef unit_value_t<meters, 3, 2> mRatio;
-
-  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<mRatio, meters>::value));
-  EXPECT_FALSE((traits::is_unit_value_t<meter_t, meters>::value));
-  EXPECT_FALSE((traits::is_unit_value_t<double, meters>::value));
-}
-
-TEST_F(CompileTimeArithmetic, is_unit_value_t_category) {
-  typedef unit_value_t<feet, 3, 2> mRatio;
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::length_unit, mRatio>::value));
-  EXPECT_FALSE(
-      (traits::is_unit_value_t_category<category::angle_unit, mRatio>::value));
-  EXPECT_FALSE((
-      traits::is_unit_value_t_category<category::length_unit, meter_t>::value));
-  EXPECT_FALSE(
-      (traits::is_unit_value_t_category<category::length_unit, double>::value));
-}
-
-TEST_F(CompileTimeArithmetic, unit_value_add) {
-  typedef unit_value_t<meters, 3, 2> mRatio;
-
-  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));
-
-  typedef unit_value_t<feet, 1> ftRatio;
-
-  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));
-  EXPECT_NEAR(5.92125984, sumf::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::length_unit, sumf>::value));
-
-  typedef unit_value_t<celsius, 1> cRatio;
-  typedef unit_value_t<fahrenheit, 2> fRatio;
-
-  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));
-  EXPECT_NEAR(2.11111111111, sumc::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
-                                                sumc>::value));
-
-  typedef unit_value_t<angle::radian, 1> rRatio;
-  typedef unit_value_t<angle::degree, 3> dRatio;
-
-  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));
-  EXPECT_NEAR(1.05235988, sumr::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::angle_unit, sumr>::value));
-}
-
-TEST_F(CompileTimeArithmetic, unit_value_subtract) {
-  typedef unit_value_t<meters, 3, 2> mRatio;
-
-  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));
-
-  typedef unit_value_t<feet, 1> ftRatio;
-
-  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_NEAR(-3.92125984, difff::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::length_unit, difff>::value));
-
-  typedef unit_value_t<celsius, 1> cRatio;
-  typedef unit_value_t<fahrenheit, 2> fRatio;
-
-  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_NEAR(-0.11111111111, diffc::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
-                                                diffc>::value));
-
-  typedef unit_value_t<angle::radian, 1> rRatio;
-  typedef unit_value_t<angle::degree, 3> dRatio;
-
-  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_NEAR(0.947640122, diffr::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::angle_unit, diffr>::value));
-}
-
-TEST_F(CompileTimeArithmetic, unit_value_multiply) {
-  typedef unit_value_t<meters, 2> mRatio;
-  typedef unit_value_t<feet, 656168, 100000> ftRatio;  // 2 meter
-
-  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));
-
-  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_NEAR(4.0, productM::value().to<double>(), 5.0e-7);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::area_unit, productM>::value));
-
-  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_NEAR(43.0556444224, productF::value().to<double>(), 5.0e-6);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::area_unit, productF>::value));
-
-  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));
-  EXPECT_NEAR(43.0556444224, productF2::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE((
-      traits::is_unit_value_t_category<category::area_unit, productF2>::value));
-
-  typedef unit_value_t<units::force::newton, 5> nRatio;
-
-  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));
-  EXPECT_NEAR(32.8084, productN::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().to<double>()),
-              5.0e-7);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::torque_unit,
-                                                productN>::value));
-
-  typedef unit_value_t<angle::radian, 11, 10> r1Ratio;
-  typedef unit_value_t<angle::radian, 22, 10> r2Ratio;
-
-  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_NEAR(2.42, productR::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(7944.39137,
-              (productR::value().convert<degrees_squared>().to<double>()),
-              5.0e-6);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
-                                                productR>::value));
-}
-
-TEST_F(CompileTimeArithmetic, unit_value_divide) {
-  typedef unit_value_t<meters, 2> mRatio;
-  typedef unit_value_t<feet, 656168, 100000> ftRatio;  // 2 meter
-
-  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));
-
-  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_NEAR(1, productM::value().to<double>(), 5.0e-7);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
-                                                productM>::value));
-
-  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_NEAR(1.0, productF::value().to<double>(), 5.0e-6);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
-                                                productF>::value));
-
-  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));
-  EXPECT_NEAR(1.0, productF2::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
-                                                productF2>::value));
-
-  typedef unit_value_t<seconds, 10> sRatio;
-
-  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));
-  EXPECT_NEAR(0.2, productMS::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::velocity_unit,
-                                                productMS>::value));
-
-  typedef unit_value_t<angle::radian, 20> rRatio;
-
-  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));
-  EXPECT_NEAR(2, productRS::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(114.592,
-              (productRS::value().convert<degrees_per_second>().to<double>()),
-              5.0e-4);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::angular_velocity_unit,
-                                                productRS>::value));
-}
-
-TEST_F(CompileTimeArithmetic, unit_value_power) {
-  typedef unit_value_t<meters, 2> mRatio;
-
-  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_NEAR(4, sq::value().to<double>(), 5.0e-8);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::area_unit, sq>::value));
-
-  typedef unit_value_t<angle::radian, 18, 10> rRatio;
-
-  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_NEAR(3.24, sqr::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(10636.292574038049895092690529904,
-              (sqr::value().convert<degrees_squared>().to<double>()), 5.0e-10);
-  EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
-                                                sqr>::value));
-}
-
-TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
-  typedef unit_value_t<square_meters, 10> mRatio;
-
-  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_NEAR(3.16227766017, root::value().to<double>(), 5.0e-9);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::length_unit, root>::value));
-
-  typedef unit_value_t<hectare, 51, 7> hRatio;
-
-  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_NEAR(2.69920623253, rooth::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(269.920623, rooth::value().convert<meters>().to<double>(),
-              5.0e-6);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::length_unit, rooth>::value));
-
-  typedef unit_value_t<steradian, 18, 10> rRatio;
-
-  using rootr = unit_value_sqrt<rRatio>;
-  EXPECT_TRUE((traits::is_angle_unit<decltype(rootr::value())>::value));
-  EXPECT_NEAR(1.3416407865, rootr::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(76.870352574,
-              rootr::value().convert<angle::degrees>().to<double>(), 5.0e-6);
-  EXPECT_TRUE(
-      (traits::is_unit_value_t_category<category::angle_unit, rootr>::value));
-}
-
-TEST_F(CaseStudies, radarRangeEquation) {
-  watt_t P_t;            // transmit power
-  scalar_t G;            // gain
-  meter_t lambda;        // wavelength
-  square_meter_t sigma;  // radar cross section
-  meter_t R;             // range
-  kelvin_t T_s;          // system noise temp
-  hertz_t B_n;           // bandwidth
-  scalar_t L;            // loss
-
-  P_t = megawatt_t(1.4);
-  G = dB_t(33.0);
-  lambda = constants::c / megahertz_t(2800);
-  sigma = square_meter_t(1.0);
-  R = meter_t(111000.0);
-  T_s = kelvin_t(950.0);
-  B_n = megahertz_t(1.67);
-  L = dB_t(8.0);
-
-  scalar_t SNR = (P_t * math::pow<2>(G) * math::pow<2>(lambda) * sigma) /
-                 (math::pow<3>(4 * constants::pi) * math::pow<4>(R) *
-                  constants::k_B * T_s * B_n * L);
-
-  EXPECT_NEAR(1.535, SNR(), 5.0e-4);
-}
-
-TEST_F(CaseStudies, pythagoreanTheorum) {
-  EXPECT_EQ(meter_t(3), RightTriangle::a::value());
-  EXPECT_EQ(meter_t(4), RightTriangle::b::value());
-  EXPECT_EQ(meter_t(5), RightTriangle::c::value());
-  EXPECT_TRUE(pow<2>(RightTriangle::a::value()) +
-                  pow<2>(RightTriangle::b::value()) ==
-              pow<2>(RightTriangle::c::value()));
-}

wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp

diff --git a/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp b/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp
index 11ac426..20d27f1 100644
--- a/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp
+++ b/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -46,7 +46,8 @@
     fail_cb_called++;
   });
 
-  GetAddrInfo(loop, [](const addrinfo&) { FAIL(); }, Twine::createNull());
+  GetAddrInfo(
+      loop, [](const addrinfo&) { FAIL(); }, Twine::createNull());
   loop->Run();
   ASSERT_EQ(fail_cb_called, 1);
 }
@@ -62,7 +63,8 @@
   });
 
   // Use a FQDN by ending in a period
-  GetAddrInfo(loop, [](const addrinfo&) { FAIL(); }, "xyzzy.xyzzy.xyzzy.");
+  GetAddrInfo(
+      loop, [](const addrinfo&) { FAIL(); }, "xyzzy.xyzzy.xyzzy.");
   loop->Run();
   ASSERT_EQ(fail_cb_called, 1);
 }
@@ -73,7 +75,8 @@
   auto loop = Loop::Create();
   loop->error.connect([](Error) { FAIL(); });
 
-  GetAddrInfo(loop, [&](const addrinfo&) { getaddrinfo_cbs++; }, "localhost");
+  GetAddrInfo(
+      loop, [&](const addrinfo&) { getaddrinfo_cbs++; }, "localhost");
 
   loop->Run();
 
@@ -90,12 +93,13 @@
 
   for (int i = 0; i < CONCURRENT_COUNT; i++) {
     callback_counts[i] = 0;
-    GetAddrInfo(loop,
-                [i, &callback_counts, &getaddrinfo_cbs](const addrinfo&) {
-                  callback_counts[i]++;
-                  getaddrinfo_cbs++;
-                },
-                "localhost");
+    GetAddrInfo(
+        loop,
+        [i, &callback_counts, &getaddrinfo_cbs](const addrinfo&) {
+          callback_counts[i]++;
+          getaddrinfo_cbs++;
+        },
+        "localhost");
   }
 
   loop->Run();

wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp

diff --git a/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp b/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp
index 16de329..76d22d9 100644
--- a/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp
+++ b/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -41,13 +41,14 @@
   auto loop = Loop::Create();
   loop->error.connect([](Error) { FAIL(); });
 
-  GetNameInfo4(loop,
-               [&](const char* hostname, const char* service) {
-                 ASSERT_NE(hostname, nullptr);
-                 ASSERT_NE(service, nullptr);
-                 getnameinfo_cbs++;
-               },
-               "127.0.0.1", 80);
+  GetNameInfo4(
+      loop,
+      [&](const char* hostname, const char* service) {
+        ASSERT_NE(hostname, nullptr);
+        ASSERT_NE(service, nullptr);
+        getnameinfo_cbs++;
+      },
+      "127.0.0.1", 80);
 
   loop->Run();
 
@@ -60,13 +61,14 @@
   auto loop = Loop::Create();
   loop->error.connect([](Error) { FAIL(); });
 
-  GetNameInfo6(loop,
-               [&](const char* hostname, const char* service) {
-                 ASSERT_NE(hostname, nullptr);
-                 ASSERT_NE(service, nullptr);
-                 getnameinfo_cbs++;
-               },
-               "::1", 80);
+  GetNameInfo6(
+      loop,
+      [&](const char* hostname, const char* service) {
+        ASSERT_NE(hostname, nullptr);
+        ASSERT_NE(service, nullptr);
+        getnameinfo_cbs++;
+      },
+      "::1", 80);
 
   loop->Run();